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| ABSTRACT 


The main problem addressed by this research is the lack of a small, low-cost integrated naviga- 
tion system to accurately determine the position of an Autonomous Underwater Vehicle (AUV) 
during all phases of an underwater search or mapping mission. The approach taken utilized an 
evolving prototype, called the Shallow-Water AUV Navigation System (SANS), combining Glo- 
bal Positioning System (GPS), Inertial Measurement Unit (IMU), water speed, and magnetic 
heading information using Kalman, low-pass, and complimentary filtering techniques. In previ- 
ous work, addition of a math coprocessor improved system update rate from 7 to 18 Hz, but 
revealed input/output coordination weaknesses in the software. The central focus of this thesis is 
on testing and programming improvements which resulted in reliable integrated operations and an 
increased processing speed of 40 Hz. This now allows the filter to perform in real time. A stan- 
dardized tilt table evaluation and calibration procedure for the navigation filter also was devel- 
oped. 

The system was evaluated in dynamic tilt table experiments. Test results and qualitative error 
estimates using differential GPS suggest that submerged navigation with SANS for a period of 
several minutes will result in position estimation errors typically on the order of 10 meters rms, 


even in the presence of substantial ocean currents. 
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I. INTRODUCTION 


A. BACKGROUND © 


Autonomous Underwater Vehicles (AUVs) are capable of a variety of overt and 
clandestine missions. Such vehicles have been proposed for inspection, mine 
countermeasures, survey, and observation. Recent research trends in underwater robotics 
have emphasized minimizing the need for human interaction by increasing AUV 
autonomy. (Yuh 95) 

The NPS Phoenix AUV is an experimental vehicle designed primarily for research in 
support of shallow-water mine countermeasures and coastal environmental monitoring 
(Healey 93, 95, Brutzman 96). The clandestine nature of the missions for which Phoenix 
was designed necessitates minimum surfaced exposure time while in the operating area, the 
ability to submerge in order to investigate targets, and a navigation system that is accurate 
enough to allow target revisit if desired. 

Many missions of the Phoenix class of vehicles can be separated into two distinct 
phases: transit and search. After being launched from an aircraft, submarine, or surface 
vessel, the AUV would execute a transit phase in order to arrive at the search area. Once 
established in the mission area, it would enter a search phase, which might include missions 
such as mine hunting, mapping, or environmental data collection. Navigation is one of the 
most important and difficult aspects of an AUV mission. Therefore, a robust, real-time 
navigation system is critical for a multi-mission capable AUV. Typically, a search phase 
would require more precise navigation than a transit phase. This could be accomplished by 


obtaining more frequent Global Positioning System (GPS) fixes, or by using Differential 











GPS (DGPS) either in real-time if available, or after mission completion using post- 
processing (Walker 96). After the search is completed, the AUV would commence a 
second transit phase and return to a recovery position. Both kinds of mission phases would 
typically involve waypoint steering, and possibly obstacle avoidance. 

An iaptoueh is described in Kwak (93) for determining the position of submerged 
detected objects by executing a “pop-up” maneuver to obtain a GPS fix. This fix is then 
extrapolated backwards to the submerged object location using recorded inertial data. 
Navigation accuracy during such a surfacing maneuver is strongly enhanced by the use of 
accurate depth information available from low-cost pressure cells (Kwak 93). However, 
this form of “aided” inertial navigation is not applicable to a surfaced or = surface AUV 
(Brown 92). 

Continuously reliable GPS reception would not be possible unless the AUV were to 
be fitted with an extensible mast mounted antenna. Extending an antenna above the effects 
of wave action is not desirable for a military application and, at any rate, would probably 
be mechanically impractical for a small AUV. As a result, any system relying solely upon 
GPS would not be sufficiently robust to provide accurate navigation information during 
surfaced or near surface operations due to intermittent reception. Therefore, inertial 
navigation is needed between periods where continuous reliable reception of GPS satellite 
signals is not possible. (Bachmann 95) 

Inertial navigation hardware is sometimes based on rotating gyros, which provide 


attitude information needed to stabilize a platform that holds acceleration sensors. The 


limiting factors to this approach include: high expense due to required precision, inordinate 








power consumption, high failure rates, and acoustic and structure-borne noise (Cox 94). 
These factors counter the Phoenix AUV philosophy of providing a low cost, general 
purpose platform capable of long-term independent operation, despite relatively small 
vehicle size (McGhee 95). Additionally, the rotating gyros now installed in Phoenix are 
aging aa mechanically unreliable. It is therefore desirable to find a solution to the AUV 
navigation and control problem not requiring such components. 

In order to achieve robust navigation, the AUV should be capable of navigating with 
GPS and/or an Inertial Navigation System (INS). GPS is capable of highly accurate 
positioning when the AUV is surfaced, while an INS can be used for submerged navigation 
and periods between GPS satellite reception. In order to ensure accurate — for a 
wide variety of missions, GPS and INS components can be combined. A favorable analysis 
of this type of navigation system was conducted in McKeon (92). The hardware and 
software architecture required for a typical mapping scenario was evaluated in Norton (94). 

Bachmann (95) made the architecture evaluated in Norton (94) a reality, and 
einai developed the first working prototype of the proposed Shallow-Water AUV 
Navigation System (SANS). The SANS was designed to overcome the problem of 
intermittent GPS satellite tracking. It is an experimental system that uses a low-cost, 
strapped-down inertial measurement unit (IMU), complemented with magnetic heading 
and water speed sensors, to enable inertial navigation between GPS fixes. This system 1s 
well suited for pop-up navigation. Finding this means of navigating near the sea surface 
provides a complete solution to the overall navigation problem associated with transiting 


an AUV to a shallow water work site, recording the position of detected submerged objects, 








and then returning to a recovery site where stored mission data can be uploaded (McGhee 
95). 

Additionally; the navigation filter developed by McGhee (95) solves the problems of 
cost and power consumption by eliminating rotating gyros and replacing them with 
sbceieatioe and angular rate sensors. This filter is implemented in SANS by Bachmann 
(95). One application of SANS is to upgrade the Phoenix navigation system. Others, 
particularly as component miniaturization continues, include marine mammal and diver 
navigation. 

With the prototype SANS having achieved favorable results in open-water, at-sea test 
trials, Walker (96) advanced the SANS to another level of maturity, re it a truly 
integrated system ready for direct application to a real-world AUV. The physically 
redesigned system includes an on-board processor and consolidated the diverse 
components into a compact unit, while improving individual component reliability and 
performance. The research reported in this thesis continues the evolution of the SANS by 
leonoaaiie software improvements to accommodate the dramatically improved 
processing speed, implementing a networking capability to monitor at-sea tests and prepare 
- for installation into the AUV, and developing a standardized calibration procedure for the 


navigation filter. 


B. RESEARCH QUESTIONS 


This thesis will examine the following research topics: 


- Evaluate the hardware and software architecture of the SANS. 


- Develop a calibration procedure for the SANS navigation filter. 

















- Evaluate the performance of the SANS navigation filter in a laboratory environment. 


- Evaluate the SANS hardware and software architecture for installation in Phoenix. 


C. SCOPE, LIMITATIONS AND ASSUMPTIONS 


This thesis reports part of the findings of the fifth year of research in an ongoing 
research project. The scope of this thesis is to evaluate SANS attitude estimation 
capabilities for eventual installation as a replacement for the older technology gyros now 
used on board the Phoenix AUV. The requirements for an ideal SANS described by Kwak 
(93) which impact this project are: 

- Low power consumption. Operation from a small external battery pack for 12 hours 

is desirable. 

- Limited exposure time. The amount of time that the GPS antenna is exposed in the 
search phase should be as short as possible. Up to 30 seconds of exposure is allowed, 
but less is better, and time between exposures should be maximized. 

- Maintain clandestine operation. The GPS antenna should present a very small cross 
section when exposed and should not extend more than a few inches above the 
surface of the water. 

- Maximize accuracy. During the search phase of the mission, system accuracy of 10 
meters or better is required following postprocessing, both while submerged and 
surfaced. 

- Total volume not to exceed 120 cubic inches. Elongated, streamlined packaging is 


preferred. 











D. ORGANIZATION OF THESIS 


The purpose of this thesis is to present the development of a prototype system intended 
to meet the mission requirements of the SANS. The term AUV is understood to include 
any small underwater vehicle (including human divers) which can easily carry such a 
compact device. The term “towfish” refers to the test vehicle used to evaluate the SANS 
during at-sea testing. 

This thesis provides an evaluation of the hardware and software used to provide 
accurate navigation for the NPS AUV. The major thrust of the thesis 1s to evaluate the 
attitude estimation capabilities of the SANS both statically and dynamically in a laboratory 
environment. 

Chapter II reviews previous work on this project as well as on GPS and INS 
navigation, AUV submerged navigation, and navigation filtering theory. Chapter [i 
provides a summary description of both the original and current SANS prototype hardware. 
Chapter IV provides a detailed description of the software architecture, including the 
navigation filter. Particular emphasis is placed on changes, additions, and updates made to 
the C++ code in support of this portion of the project. Chapter V is a description of the 


experiment design and an analysis of the experimental results. Finally, Chapter VI presents 


the thesis conclusions and provides recommendations for future research. 








Il. SURVEY OF RELATED WORK 


A. INTRODUCTION 


Autonomous Underwater Vehicles (AUVs) have the potential to be used in an efficient 
and cost effective manner in a variety of missions involving military and non-military 
applications. Accurate navigation is one of the most important capabilities supporting 
AUV mission effectiveness. Many possible AUV missions, such as mine hunting, require 
a high degree of navigation accuracy. This chapter will discuss some of the possible AUV 
navigation solutions. 

Navigation systems are generally categorized by whether they are based on external 
signal reception or internal sensors. External-signal-based navigation systems, such as 
Loran, Omega, and Global Positioning System (GPS), are limited to determining position 
only while the receiver is exposed to the signal. Loran and Omega are relatively inaccurate 
compared to GPS. While Loran covers most of the northern hemisphere, it has almost no 
coverage in the southern hemisphere (Bowditch 84). GPS provides an attractive, 
affordable system for the Ser portion of an AUV mission because it is capable of 
world-wide coverage with a high degree of navigational accuracy. 

Intemnal-sensor-based navigation can be implemented as a self-contained unit which 
can be composed of various types of equipment such as inertial measuring units (IMUs), 
acoustic transponders, or geophysical map comparison. All sensors are subject to some 
amount of error, which may compound to unacceptable levels for some AUV missions if 
not accounted for. Each of these components also has unique disadvantages. Acoustic 


transponders must be pre-deployed at precisely known locations and may require costly 











maintenance. Geophysical map. interrogation requires a precise bottom contour map be 
previously stored in the AUV’s computer. IMU-based navigation is prone to sensor drift, 
which if left uncorrected, can become very large. However, it has advantages relative to 

the other-navigation options due to a lack of dependence on external signals and no 


requirement to transmit any signals which might reveal its presence. 


B. GPS NAVIGATION 
The Navigation Satellite Timing and Ranging (NAVSTAR) Global Positioning System 


(GPS) is a space-based radio positioning, navigation and time-transfer system sponsored 
by the U.S. Department of Defense (DoD). It was originally intended to provide the 
military with precise navigation and timing capabilities (Parkinson 80). The system is 
designed to provide 24-hour, all-weather navigation anywhere on earth. It is comprised of 
24 satellites in 22,200 km orbits that are inclined at 55° to the earth’s spin axis, with 12 
hour periods. The pallies broadcast two L-band frequencies: L1 (1575.4 MHz) and L2 
(1227.6 MHz). Navigation and system data, predicted satellite position (ephemeris) 
information, atmospheric propagation correction data, satellite clock error information, and 
satellite health data are all superimposed on these two carrier frequencies. (Logsdon 92, 
Wooden 85) 

There are two different navigation services available from the GPS satellites depending 
on the type of receiver being used: the Standard Positioning Service (SPS), and the Precise 
Positioning Service (PPS). The SPS is based on receiving the L1 carrier signal, which is 


broadcast with an intentional inaccuracy called Selective Availability (SA). SA limits 


world-wide navigation to 100 m horizontal accuracy with a 95% confidence level (Logsdon 








92). PPS is based on the L2 signal. It is limited to U-S. and allied military, and specific 
non-military uses that are in the national interest. Access to PPS is restricted by use of 
special cryptographic equipment. PPS provides the highest stand alone accuracy: 16m 
Spherical Error Probable (SEP), a velocity accuracy of 0.1 m/sec, and a timing accuracy of 
better than 100 nanoseconds. (Logsdon 92, Wooden 85) 

Civilian cians have determined a way to improve the accuracy of the SPS in order 
to take full advantage of GPS precision without having access to cryptographic equipment. 
This method, called Differential GPS (DGPS), provides a way of working around the 
inaccuracies of the SPS. It may be used in real-time or during post-processing. The general 
idea is to place a receiver at a surveyed stationary site. The receiver 2 then able to 
determine the difference between its actual position and its computed GPS position, and 
broadcast the resulting pseudorange (distance to satellite) corrections to any DGPS capable 
receivers. Real-time differential processing can reduce the typical 100 m accuracy of the 
SPS to 2-4 m regardless of the status of SA (Logsdon 92). It is also possible to record the 
raw PPS or SPS GPS information for later comparison to a known geographical site using 
post-processing. Precise procedures can be used to reconstruct extremely accurate 
positioning information, typically in the submeter range. Table 1 shows a comparison of 
expected GPS accuracies. 

The size and cost of GPS receivers have decreased drastically as GPS technology has 
matured. Miniaturization is continuously progressing while maintaining or increasing GPS 
receiver performance capability. Since as early as 1992, the GPS industry has been able to 


produce receivers that are essentially a single printed circuit board. Souen (92) reports 
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TABLE 1: Expected RMS GPS Accuracy Levels (Logsdon 92) 


that the Furuno GPS receiver module LGN-72 is an eight-channel receiver implemented on 
a single printed circuit board measuring 100 mm x 70 mm x 20 mm and requiring only 2 
W of power. 

There is currently a performance trade-off associated with the miniaturization of GPS 
receivers. For instance, Trimble offers the PC Card 110 GPS miniature receiver in the form 
of a Personal Computer Memory Card International (PCMCIA) interface. This credit card- 
sized device simply slides into any laptop, most palmtops, or pen-based computers 
compliant with PCMCIA (release 2.0). It is capable of tracking eight satellites using three 
channels. However, because it does not have an allocated channel for each of the satellites, 
it does not use a continuous tracking scheme. This degrades its acquisition time 
performance. In order to reduce receiver size, manufacturers often reduce the number of 
channels on the receiver. GPS receivers in this configuration are called “sequencing” 
receivers (Logsdon 92). Sequencing receivers utilize a time-sharing technique to “dwell” 
on each satellite for a brief interval before switching to the next satellite in the sequence. 
They have a typical acquisition time of about two minutes. Continuous tracking GPS 
receivers have typical acquisition times of about 30 seconds or less. However, their larger 


number of receiver channels results in a less compact size. Given this trade-off between 
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size and performance, the choice of GPS receiver must be made with the particular 
application in mind. A sequencing receiver offers an adequate compromise for applications 
| such as mobile navigation that are not so dynamic. However, if the application requires a 
short time to initial acquisition, the most viable option is the continuous tracking receiver. 
GPS is aa obvious choice for AUV navigation given the level of miniaturization and its 
excellent accuracy performance. 

One manner of using GPS to locate an AUV is to place buoys with GPS receivers at 
appropriate locations. These buoys would translate the GPS signal and retransmit an 
underwater acoustic signal. The AUV would then determine its position via ranging and 
position fixes to the buoys. Youngberg (91) suggests that the GPS enienne receiver, 
processing and ccnuel subsystem, acoustic transmitter, battery power, and homing beacon 
could all be contained in a buoy measuring 123 mm diameter x 910 mm long and weighing 
5-15kg. A simulation which showed the feasibility of this approach is presented in Leu 
(93). The simulation consisted of several sonobuoys spaced one kilometer apart. Due to 
ceenaintes in buoy position caused by wave action and variations in altitude, the study 
proposed the use of Kalman filtering techniques to combine the outputs of an accelerometer 
and DGPS to enhance accuracy. Each GPS buoy would essentially act as a GPS satellite 
and broadcast its position via spread spectrum acoustic signals used by the AUV for 
ranging. This technique would eliminate the requirement to predeploy a surveyed 
transponder field. 

Another possible method for using GPS to determine the AUV’s position is to 


physically mount the GPS antenna and receiver on-board the AUV. For areas covered by 
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DGPS service, this has the advantage of making the system self-contained. One major 
concem would be that the GPS receiver would be unable to acquire satellites in a timely 
manner due to splash effects on the antenna. However, Norton (94) describes both static 
and dynamic test results which show that a submersible system is able to meet the accuracy 
and aie requirements of the mission, even while being splashed by wave wash. Therefore, 


this method was adopted in the SANS configuration. 


C. INS NAVIGATION 


Inertial navigation is essentially a complex method of dead reckoning. Its purest form 
involves no outside references to fix position. All position data is calculated relative to a 
known starting point. An inertial navigation system (INS) continuously measures three 
mutually orthogonal acceleration components using accelerometers. These measurements 
are taken in short time increments and multiplied by elapsed time in order to determine an 
estimate of instantaneous velocity. The three-dimensional change in position can then be 
determined by integrating respective velocities over time. (Bachmann 95) 

The primary drawback of any INS is the tendency for small sensor drift rates to 
accumulate as errors over time. Without outside references for correction, these errors 
grow relentlessly and eventually lead to large errors in the estimated position. Highly 
accurate inertial navigation systems can be constructed, but they are large, costly, and 
complex (Touhy 93). Size alone makes them unacceptable for the SANS. A compromise 
solution to meet SANS requirements is to integrate a low-cost, miniature INS with GPS. 
In such a system, GPS will provide the INS with the periodic position fixes necessary to 


correct slowly building INS errors. 
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The acceleration measurements required by an INS can be made by several types of 
IMUs. There are two fundamental categories: gimbaled and strapdown. Due to their large 
size and power requirements, gimbaled systems are not suitable for the SANS. Ina 
strapdown unit, three mutually orthogonal accelerometers and three angular rate sensors 
are rniguntes parallel to the three body axes of the vehicle. Linear accelerations and 
rotational velocities are continuously measured. Strapdown systems are smaller and 
simpler than simbaled systems, but necessitate much larger computational capabilities. 


(Logsdon 92) 


D. INTEGRATED GPS/INS NAVIGATION 


SPS mode GPS navigation could be used to adequately perform both the transit and 
search phases of an AUV mission. During surfaced transit phases, non-differential SPS, a 
water speed sensor, and a magnetic compass would provide the primary source of 
navigation data. In order to utilize GPS as a meaningful correction to a low-cost INS 
system, periods between GPS fixes during the transit phase must not exceed the time in 
- which the INS error has accumulated to an amount comparable to the horizontal accuracy 
of SPS (100m) (Bachmann 95). The search or mapping phases of an AUV mission would 
require the vehicle to maintain a more accurate navigational picture, both submerged and 
on the surface. This would necessitate the use of periodic differentially corrected GPS 
information in order to keep the INS system accurate while submerged. This differential 
correction could be provided in real-time during overt missions along friendly shores where 
a DGPS reference signal is available, or during mission post-processing following a 


clandestine mission. 











Integration of GPS and INS into a single system can produce continuously accurate 
navigational information even when using relatively low-cost components. This 
integration not only allows periodic reinitialization of the INS to correct accumulated 
errors, but can also (with the aid of Kalman filtering techniques) improve the performance 
of the INS between fixes. Complementary filtering of acceleration data with additional 
sensor information such as water speed and heading will further improve system accuracy. 
Overall, an integrated system will provide improved reliability, smaller navigation errors, 
and superior survivability. (Logsdon 92) 

Kalman filtering is a method of combining all available sensor data, regardless of their 
precision, to estimate the current posture of a vehicle (Cox 90). The filter ipiiaiiis a data- 
processing algorithm which minimizes the error of this estimate statistically using currently 
available sensor data and prior knowledge of system characteristics. Each piece of data is 
weighted relative to data from other system components based upon the expected accuracy 
of the measurement it represents. In a complementary filter, low-frequency data, which is 
trusted over the long term, and high-frequency data, which is trusted only in the short term, 
are used to “complement” each other providing a much better estimate than either can 
alone. (Brown 92) 

Bachmann (95) demonstrated the use of the complementary filter technique by 
combining low-frequency orientation data from accelerometers and a magnetic compass 
with high-frequency angular rate information to estimate heading and attitude. 
Intermediate position results were obtained by integrating high-frequency water-speed 


data. GPS data was used to reinitialize the system each time a fix was obtained and to 
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develop an error bias, expressed.as an apparent ocean current. The current was utilized to 
correct the system between GPS fixes. The concept of using a relatively inexpensive IMU 
with limited accuracy, coupled with differentially-corrected GPS, has proven to be a viable 
solution to the challenge of shallow-water AUV navigation. (Bachmann 95) 

The above conclusion has been independently duplicated in Wolf (96). Utilizing an 
integrated GPS/INS system using the same Systron-Donner IMU used in SANS, but 
without incorporating DGPS, accuracies in attitude of better than 0.2’ in roll and pitch and 
0.3° in azimuth were achieved. Specific results from those tests, along with static tests 
indicating the SANS software filter (described in Chapter IV) response to IMU inputs are 


discussed further in Chapter VI, System Testing. (Wolf 96) 


E. AUV SUBMERGED NAVIGATION 


There are many techniques available for submerged navigation, including dead 
reckoning, inertial, electromagnetic, and acoustic navigation. With acoustic navigation, 
time of arrival and direction of propagation of acoustic waves are the two principal 
measurements made. A wide variety of acoustic navigation systems have been developed 
for underwater vehicle use. They are typically divided into long, short, and ultrashort 
baseline systems. All involve the use of acoustic beacons or receivers whose positions 
must be known to an accuracy somewhat better than the desired vehicle localization 
accuracy (Tuohy 93). Unfortunately, most acoustic navigation systems require major 
expeditions for their accurate set-up and periodic maintenance. This makes them 
expensive, and in many ways reduces the level of autonomy achievable by an AUV. Also, 


acoustic methods are affected by changes in the speed of sound in the ocean and suffer from 
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refraction and multipath propagation problems in restricted shallow water coastal and ice- 
sovered areas. (Tuohy 93) 

There are various alternative submerged navigation methods not dependent upon the 
aid of external signals. Charge Coupled Device cameras, laser scanning, or variations in 
the satis magnetic field can aid in determining position (Bergem 93). Position can also 
be estimated by the double integration of acceleration as sensed by an IMU. 

Doppler sonar or correlation velocity log sensors can be utilized to determine speed 
through the water or over the ground. Doppler velocity logs utilize the physics of 
frequency shifts in the sound waves of sources and receivers with relative radial motion. A 
critical assumption for two-way transmission in the ocean is that the deed scatterers, 
(small particles and plankton) uniformly populate the environment, and at the average 
move at the same horizontal velocity as the water. Correlation velocity logs, on the other 
hand, use reflections from the sea bottom, even at great depths, and on-board sensor arrays 
to detect forward and lateral motion occurring between sonar pings. (Gordon 96) 

Doppler technology has been redesigned as the Acoustic Doppler Current Profiler 
(ADCP). The ADCP measures water velocity more accurately, and allows measurement 
in range cells over a depth profile. Throughout the 1980’s, ADCPs were further improved 
by production of self-contained, vessel-mounted, and direct-reading models, and by the 
addition of broadband capability in 1991. Broadband ADCPs take advantage of having 
typically 100 times as much bandwidth for measuring velocity as the original, narrow- 


bandwidth models, reducing variance nearly 100 times. (Gordon 96) 
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Broadband Doppler processing computes the phase change of propagation time delay. 
Since longer propagation times provide greater accuracy, but incur phase changes beyond 
360°, a mathematical autocorrelation function resolves ambiguity and allows transmission 
of a series of coded pulses within a single long pulse. Multiple beams are utilized to obtain 
velocity in three dimensions, under the assumption of uniform currents across layers of 
constant depth. Non-homogenous current layers produce large velocity errors. (Gordon 
96) 

ADCP single-ping random or short-term error may range from just a few mm/s to as 
much as 0.5 m/s, depending on internal factors such as frequency, depth cell size, number 
of pings averaged together, and beam geometry. Since this random er uncorrelated 
from ping to ping, the sealed deviation of the velocity error can be reduced by the square 
root of the number of pings through averaging. Although averaging can greatly reduce the 
relatively large, single-ping error, at a certain point it fails to improve on overall error as 
the random error becomes smaller than the bias. (Gordon 96) 

The bias is typically less than 10 mm/s and depends on factors such as temperature, 
mean current speed, signal/noise ratio, and beam geometry. It is not yet possible to 
measure ADCP bias and calibrate or remove it in post-processing. External error factors 
include turbulence, internal waves, and ADCP motion, and can dominate internal errors. 
While the technology behind the ADCP is impressive and bears serious consideration for 
future small AUV navigation development, the combination of relative affordability and 


unpredictable bias make it a les attractive option for the SANS application. (Gordon 96) 








For covert missions, an AUV may not be able to refer to external signals while 
sibmerzed. In this case, the system must rely on some sort of dead reckoning. Modern 
dead reckoning systems typically use magnetic or gyroscopic heading sensors, and a 
bottom or water-locked velocity sensor (Grose 92). The presence of an ocean current will 
add a scisey component to the vehicle which is not detected by a water speed sensor. In 
the vicinity of the shore, ocean currents can exceed two knots (Tuohy 93). Using dead 
reckoning with currents which are relatively large in relation to the typical 4-6 knot speed 
of an AUV can produce extremely inaccurate results (Tuohy 93). This inaccuracy 
represents the central challenge of AUV submerged dead reckoning navigation. 

There are many techniques for measuring acceleration and angular rates. These include 
using ring laser and fiber optic gyros, rotating mass gyros, vibratory rate sensors, and high 
performance IMUs. Inertial grade IMUs typically contain three angular rate sensors, three 
precision linear accelerometers and a three-axis magnetometer. The acceleration 
measurements required by an INS can be made by several types of IMUs. All of these 
sensors are subject to drift errors which relentlessly increase with time. High quality 
sensors are subject to less drift, but can cost up to $100,000 (Tuohy 93), making them 
unattractive for small AUVs. 

McKeon (92) proposes a combination of GPS and INS to allow an AUV to determine 
position information. While submerged, the AUV uses a low-cost inertial navigation 
system. However, when on the surface, the vehicle has access to GPS information. GPS/ 
INS information could be combined with Kalman filter techniques to reduce errors during 


the next dive sequence as simulated in Nagengast (92) and demonstrated in McGhee (95). 
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The system described in McGhee (95) senses linear accelerations and angular rates with 
respective sensors and processes the data in a twelve state Kalman filter, resulting in an 
estimated position. A mechanical water speed sensor and a magnetic compass are added 
to complement acceleration and angular rate data and further enhance navigation accuracy. 
The wee states can be divided into seven continuous-time states (three Euler angles, two 
horizontal velocities, two horizontal positions), two discrete-time states derived from the 
DGPS fixes (estimated east and north current), and three angular rate sensor bias estimates, 
(subtracted from the output of these sensors). The DGPS fixes occur aperiodically 
whenever the vehicle surfaces and is able to acquire a sufficient number of satellites. 


(Bachmann 96) 


F. NAVIGATION FILTER THEORY 


The inherent sensor measurement errors plaguing inertial measurement systems may 
be partially compensated for, but never eliminated. Drift is the tendency of bias errors in 
the angular rate sensors of the inertial platform to cause relentlessly increasing orientation 
measurement errors. The single integration of a bias-ridden angular rate signal will cause 
a steady build-up of error over time. This leads to an incorrect estimation of the body 
orientation relative to the earth-fixed coordinate system and a corresponding body position 
estimate error. Angular rate sensor biases typically change unpredictably over time, 
making a simple, complete compensation impossible. (Frey 96) 

Standard inertial navigation procedures utilize fix updates if an alternative method of 
determining instantaneous orientation exists. Drift is compensated for by seradie 


adjustments of the inertial system to the external reference, returning the bias error 
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accumulation to zero. Short fix intervals then result in relatively insignificant bias errors. 
Higher quality angular rate sensors typically have lower bias errors and correspondingly 
longer fix intervals. (Frey 96) 

Linear acceleration sensor drift errors are compounded by the double integration of the 
linear seeieaeon measurements to obtain position data. This results in a position estimate 
in error proportional to time-squared, rather than simply time. This error may be similarly 
compensated. However, given the same sensor quality, the fix interval needed to maintain 
comparable accuracy will be much shorter than that required for the angular rate sensor bias 
compensation alone. (Frey 96) 

Discrete low pass filter theory provides a method for obtaining a rate bias estimate. 
Such filters may be represented by a signal-flow graph (SFG), which is a simplified version 
of a block diagram. The SFG was introduced by S. J. Mason for the cause-and-effect 
representation of linear systems that are modeled by algebraic equations (Kuo 95). ASFG 
may be defined as a graphical means of portraying the input-output relationships between 


the variables of a set of linear algebraic equations, or simply 
Eq (2.1) 
output = Y gain X input 
Corresponding block and signal flow graph diagrams for a single input discrete low pass 


filter are shown in Figures 1 and 2 below. 


- 
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Figure 1: Discrete Low Pass Filter Block Diagram 


approximated by 
xy be: i(t)At “1 y 





Figure 2: Discrete Low Pass Filter Signal Flow Graph 


In this diagram, p™' stands for the time domain integration operator, and tau 1s the 


relaxation time constant. Directly from Figure 2, 


Eq (2.2) 
x,(t+At) = X(t) + x(t) At 


OT 
Eq (2.3) 


new output = old output + mper coup at 


This is the classic relationship describing a low pass filter (McGhee 96). Rewritten, 


Equation 2.3 becomes Eq (2.4) 


(input —x,(t))At 


x,(¢+At) = x,(t)+ . 


Which can also be written 
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Eq (2.5) 


At At 
X (t+ At) = X4(t) + input— — x ()— 
and, finally 
Eq (2.6) 
x,(t+At) = (01 ~ St) input 


or, in more common terminology, and the terms used in the SANS code 


Eg (2.7) 
new output = outputWeight x old output + input x sample Weight 


The above general result can be applied to the SANS system for rate sensor bias 
estimation. In this case, the signal used for attitude estimation is the raw rate sensor reading 
with the estimated bias subtracted. An alternative formulation is to add the negative of the 
bias to the sensor reading. This formulation is derived similarly, and is implemented in the 


SANS code as, 


Eq (2.8) 
new negative bias = biasWeight x old negative bias — input x sample Weight 


In this form, the bias estimation integrator is initialized to a negative average value and the 


bias is then added to the sensor input. 
G. SUMMARY 

Many approaches to the problem of AUV navigation have been devised. New ones 
are still emerging and technological improvements are improving current approaches. 
Choices range from simple dead reckoning, to systems which use acoustic information 
from floating or stationary transponders, to complex systems which use sophisticated IMUs 


and GPS receivers combined with Kalman filtering techniques. Most of the described 
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approaches can only be used in very specialized applications. Most are also limited by 
dependence on previously deployed external means and by some requirement to actively 
exchange data with those means. The preferred method of many developers is the acoustic 
approach. However, most of these systems have a higher degree of complexity and 
ieenaiatinl on external means than the system implemented in McGhee (95). 

It can be seen that high accuracy and other design goals for an inertial navigation 
system ae achievable. But clearly, the cost increases rapidly with the degree of 
sophistication and the desired precision. From this point of view the NPS Phoenix AUV, 
described in Healey (94), together with the SANS navigation system developed by 
Bachmann (95), McGhee (95), Steven (96), and Walker (96), promises ers a very 
effective means for achievement of clandestine missions in shallow water by a small AUV. 

The remainder of this thesis continues an ongoing experimental study pertaining to the 
development of the SANS system and associated problems. The current system under 
evaluation is of small physical size and relatively low cost. The IMU selected is 
representative and has limited accuracy, so additional water-speed and magnetic heading 
information is required. Accelerometers are used mainly to derive low frequency attitude 
information, and are not utilized for velocity or position estimation for periods of more than 
a few seconds. 

Previous research on the prototype SANS has produced test results and qualitative 
error estimates which indicate that submerged navigation accuracy comparable to GPS 
surface navigation is attainable (Bachmann 95). The research goal of this thesis is to refine 


the hardware and software configuration to allow more accurate submerged navigation, and 
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to develop the SANS into a self contained system capable of being internally or externally 


attached to any AUV, delivering regular, accurate, real-time position updates. 
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UI. SYSTEM HARDWARE CONFIGURATION 


A. INTRODUCTION 


Bachmann (95) describes the initial prototype in the ongoing development of the 
SANS. Walker (96) redesigned the original prototype to consolidate components in one 
integrated system. In addition, he presented an evaluation summary of the original 
prototype hardware, with particular emphasis on the noise characteristics of the Systron- 
Donner MotionPak IMU, which is retained in the SANS. 

Figure 3 presents a block diagram for the hardware making up the redesigned SANS. 
Figure 4 presents a photograph of the SANS components fully assembled into their testing 
configuration. The project box in which the components are currently mounted is an 
interim solution. A more permanent, water-tight, streamlined housing is currently in 
development. 

This configuration is significantly different from the previous prototype presented in 
Bachmann (95). The SANS components are no longer separated; all components are 
physically located in one self-contained package. When joined with its accompanying 
power source (a 12 VDC battery), the complete system can now be strapped-down to a tilt 
table or inserted into a towfish for at-sea testing. In its current configuration, the SANS has 
its processor and GPS/DGPS components “on-board,” thus no longer requiring the transfer 
of sensor data via modem to an external processor or GPS/DGPS receiver. (Bachmann 95, 
Walker 96) 

The SANS processor is linked with an external processor via a DOS TCP/IP network 


connection to allow for human monitoring and interaction during the course of an 
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Figure 3: Redesigned SANS Hardware Configuration (Walker 96) 


experiment. This external processor’s only function is to maintain a remote control 
session with the SANS processor and receive its attitude and position updates. Unlike the 
original SANS proof of concept design presented in Bachmann (95), the SANS now 
maintains the capability to on-board process its own data and interface with any other 
higher-level processor via a network. This capability will directly enable smooth 
incorporation of SANS into the Phoenix architecture. This chapter will summarize the 


hardware component capabilities realized in Walker (96). 
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B. HARDWARE DESCRIPTION 


1. Computer 
The on-board processor is an Extremely Small Package (E.S.P.) Cyrix 486SLC DxX2 


50 MHz computer, pictured in Figure 5. Itis specifically designed to offer off-the-shelf 
PC-compatible solutions in space and/or power constrained environments. This particular 
E.S.P computer possesses a total of eight modules which perform various system tasks. 
Together, the processor and its accompanying modules provide a small, low-power system 
with system performance comparable to a standard, desk-top type system. (MAXUS 95, 


Walker 96) 


BSc 





Figure 5: E.S.P. 486SLC DX2 50 MHz Computer (Walker 96) 


The CPU Module provides the processing capability, the interface for a standard 
keyboard, the Flash PROM containing the system BIOS, and memory and bus controller 
logic. The DC-DC Power Module provides for all the system power requirements up to a 
maximum 35W total output. It accepts an unregulated 12 V DC and provides the required 


+5, +12, -12, and -28 V DC to power various system components and optional peripherals 
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(i.e., an external floppy and hard drive, as is used in the tilt-table test configuration). 
(Maxus 95, Walker 96) 

The VGA Adapter Module provides the interface to operate an external VGA monitor. 
A PC I/O Module provides for two Serial ports and one parallel I/O port. It also provides 
two type-II PCMCIA sockets which conform to PCMCIA Release 2.01 standard. These 
two ports can be used for a variety of compatible devices (i.e., Ethernet Adapter, Modem, 
GPS Receiver, etc.). This module was included in the current design to provide additional 
secondary storage in the form of PCMCIA SRAM cards, as well as to enable possible 
future expansion. An Ethernet Module provides the SANS with an external ethernet 
interface. (Maxus 95, Walker 96) | 

The Analog to Digital (A/D) Module provides 8 differential or 16 single-ended input 
channels at 12-bit resolution. In its current configuration, the A/D module samples only 8 
of the available 16 single-ended channels. It features a single-channel maximum sampling 
rate of 333 KHz, and an input range from +/- 1.25mV to +/-10V (MAXUS 95). The A/D 
_— provides a 34-pin external connector (J3) to which developers can connect their 
input signals. (Walker 96) 

The DRAM Module provides for high-speed (70ns) memory storage available in 2, 4, 
6, 8,or 16MB capacities (MAXUS 95). This module is to the E.S.P. as a hard disk is tu a 


standard desk-top PC. (Walker 96) 


2. Inertial Measuring Unit 


The inertial navigation component of the SANS is provided by a Systron-Donner 


Model MP-GCCCQAAB-100 “MotionPak” inertial sensing unit, pictured in Figure 6. This 
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self-contained unit provides analog measurements in three orthogonal axes of both linear 


acceleration and angular velocity. It consists of a cluster of three accelerometers and three 


“Gyrochip” angular rate sensors. (Walker 96) 








Figure 6: Systron-Donner Inertial Measuring Unit (Bachmann 95) 


3. GPS/DGPS Receiver Pair 

The GPS/DGPS receiver used is the ONCORE 8-channel receiver which incorporates 
an imbedded DGPS capability (Oncore 95). The receiver is capable of tracking up to eight 
satellites simultaneously. It can provide position accuracy of better than 25 meters 
Spherical Error Probable (SEP) without Selective Availability (SA), and 100 meters (SEP) 


with SA. Typical Time-To-First-Fix is 18 seconds with a typical reacquisition time of 2.5 


seconds (Oncore 95). This receiver meets or exceeds the capabilities of the receiver 
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described in Norton (94), which, under normal operating conditions, met the accuracy and 
time requirements of the SANS project. Norton (94) also demonstrated that a receiver with 
these qualities will perform well when using an antenna that is located on or near the sea 
surface; as is necessary during a clandestine mission. Figure 7 shows the ONCORE GPS/ 


DGPS receiver used in the SANS project. (Walker 96) 





Figure 7: ONCORE GPS/DGPS Receiver (Walker 96) 


4. Compass 

The compass used in the SANS project is a Precision Navigation model TCM2 
Electronic Compass Module. This compass does not employ the mechanical gimbal 
technology utilized in the compass described in Bachmann (95), but rather employs a three- 
axis magnetometer and a high-performance two-axis tilt sensor in a small form-factor 
(TCM2 95). The TCM2 compass is capable of providing readings of pitch, roll, and 
surrounding magnetic field strength in addition to heading. The TCM2 provides greater 
accuracy by calibrating (performed by the user) for distortion fields in all tilt orientations, 
providing an alarm when local magnetic anomalies are present, and giving out-of-range 


warnings when the unit is being tilted too far (TCM2 95). (Walker 96) 
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5. Other Components . 


The water speed sensor and the depth sensor are those described in Bachmann (95) and 
therefore are not depicted in Figure 5. The GPS antenna shown in Figure 5 is an active 
antenna, which was selected for its performance and low profile. Because the E.S.P. 
Ethernet module’s output media type is AUI, a standard AUIJ-to-BNC media converter is 
employed to allow the use of durable RG-58 coax cable to span the roughly 100m distance 
required while pulling the towfish behind a towing vessel. The GPS/DGPS Interface box 
is nothing more than an adapter to interface the GPS receiver signal with the serial port of 
the E.S.P. computer. (Walker 96) 

Based on the analysis given in Walker (96), the 2-pole anti-aliasing Bessel filters used 
in Bachmann (95) were replaced with new low-harmonic distortion filters. These come 
factory tuned to a user-specified corner frequency of 10 Hz, require no external components 
or adjustments, and operate with a dynamic input voltage range from non-critical +/- 5V to 
+/-18V power supplies (Frequency Devices 96). To implement these filters into the SANS, 
a double-sided printed circuit board was designed and machined to receive all six filter 
DIPs, as well as three quad op-amp LM324 DIPs configured as voltage-followers to 
provide input and output circuit protection. (Walker 96) 

To provide for the requisite +/-15 VDC, a DATEL model BWR-15/330-D12 DC-DC 
Converter is used to convert the unregulated 12 VDC battery input into regulated +/-15 
VDC needed to power the low-pass filter circuits and the IMU. This converter features 


over-current and short-circuit protection, a compact form-factor, and high reliability at a 


32 














minimum efficiency of 82%. It employs switching regulator technology, which minimizes 
heat generation and current usage. (DATEL 95, Walker 96) 

Physically connecting the IMU, Low-pass Filters/DC-DC Converter PCB, the Analog- 
Digital Converter, input power, water speed sensor, and depth sensor, is a 25-strand flat 
ribbon cable. This type of cable was chosen to allow all system components to be easily 


interconnected. (Walker 96) 


C. SUMMARY 


The SANS design described in this chapter is significantly different from that 
described in Bachmann (95). The processing capability, along with the GPS/DGPS 
receiver, 1s now on-board the SANS, making it completely self-contained. The only 
external link is a DOS ethernet environment to a remote PC utilized for test monitoring 
purposes. The IMU sensor data, after low-pass filtering, along with water speed and depth 
data, are converted from analog to digital form, with 12-bit resolution, and then passed to 
the processor. GPS data is passed separately to the processor, which computes updated 
attitude and position information to be exported over an ethernet socket. The hardware for 
this version of the SANS was chosen to comply as far as possible with the requirements set 
forth in Kwak (93). Though there are many possible choices of hardware for each of the 
components in Figure 4, trade-offs between accuracy, size, power requirements, and cost 
have been considered. As further advances in miniaturization are made, accuracy will 
continue to increase while price and size decrease, thus making it easier to meet the 
challenges of the SANS baseline requirements. The next chapter of this thesis will describe 


the software which supports this hardware configuration. 
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IV. SOFTWARE DEVELOPMENT 


A. INTRODUCTION 


The purpose of the SANS software is to control some of the individual hardware 
components, to control input/output interface communications between the components, to 
assimilate all of the incoming data, and to implement a twelve state navigation filter. This 
chapter will review the software structure inherited from Bachmann (95) and Walker (96), 
and will concentrate particularly on the changes made to accommodate the greatly 
improved processing speed that Walker (96) made possible. 

The code is written in C++ and is designed for use on an IBM-compatible personal 
computer using the Borland version 3.1 compiler under DOS 5.0. This project code choice 
has proven to complicate the integration of the hardware interfaces. Additionally, the dated 
software compiler formats and DOS system calls make the code specific to this application 
only and increases the difficulty of troubleshooting or implementing changes. Although 
most of the code is transportable to other C++ compiler environments, the interrupt 
processing and input/output communications control uses obsolete type declarations and 
function calls to the rapidly aging operating system. 

This limitation could easily be resolved in future project work in either of two ways. 
Utilizing a Borland version 5.0 compiler with updated communications code would allow 
continued use of a traditional IBM-compatible environment. Converting those sections of 
code to be compatible with Unix environment compilers could also be implemented on the 


PC under the Linux operating environment. | 
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The software design instantiates objects corresponding either to the individual 
hardaare components or to the purpose accomplished, in a straightforward manner. The 
class and object relationships are shown in Figure 8. All of the concrete classes depicted 
are specifically instantiated by the class instance above them, in descending chronological 
order is the program is initiated. All are instantiated as a single object, named as shown. 
There is no need in this application for extensive polymorphism. The serialPortClass and 
bufferClass classes are abstract parent classes containing the common definitions and 
functions from which the specific compassPort, compassBuffer, gpsPort, and gpsBuffer 
classes inherit. The stampedSample object, defined in the main program’s header file, 
contains the latest update of all pertinent navigation information. Therefore, it is the object 
which is passed between the class objects. Other objects which support the calculations are 
structures to hold such things as position in the various formats. For simplicity, they are 
not shown in Figure 8. 

This architecture represents a substantial change from the original design, while 
retaining most of the functionality. As the project evolved, it was determined that much of 
the flexibility originally envisioned did not prove to be necessary. This includes features 
such as the capability to instantiate an array of serial ports, or a need for a wide variety of 
buffers for the data received through the serial ports. 

The above features were included in the original object oriented design approach, but 
have been streamlined to a more specific, less complicated structure. Specifically, the 
portbank and bytebuffer classes have been removed. Only two serial ports are required, for 


the compass and gps interfaces, respectively. The serial port code was modified, and the 
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Figure 8: SANS Code Classes and Objects 











buffered serial port class has been specialized to a compassPort class and a gpsPort class, 
while iatne the same basic function. This resulted in the compassPort and gpsPort 
classes representing a kind of serial port, similar to the way the compBuffer and gpsBuffer 
classes already were a kind of bytebuffer and continue to be a kind of buffer. This 
simplified the class membership hierarchy and variable passing across class lines. The 
specific nature of the application made efficiency a higher priority than general 
applicability. 

Other improvements included the addition of configuration files containing such data 
as gain settings to allow repeated testing without the necessisity of recompiling after every 
change. The increased processing speed overwhelmed the DOS operating sisal ability 
to print information to the user’s screen in real time, so an interval was added that reduced 
screen output to a more usable human rate that also reduced input/output conflicts. All 
screen output and data writing to files were consolidated to single points to further simplify 
exchanges. And finally, some error checking was added to ensure such things as proper A/ 


D converter channel initialization. 


B. SOFTWARE FILTER 


The purpose of the software filter is to utilize IMU, heading, and water-speed 
information to implement an INS, and then to integrate this with GPS information. This 
results in a single system which can produce continuously accurate navigational 
information in real time. The filter mitigates the effects of sensor inaccuracies (inherent, 
electronic noise, and transitory), ocean current (the largest single factor affecting AUV 


navigation), dynamic model uncertainty, measurement errors, and calculation errors. 
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Kalman filtering techniques are-used to implement the INS using DGPS fixes as “error-free 
data”. This allows periodic reinitialization of the INS to correct accumulated dnft and 
development of error biases. All sensor data is logged in raw form for post-mission 
processing. (Bachmann 96) 

Fiawe 9 is a data flow diagram for the SANS software filter. On this diagram, R 
represents a rotation matrix and T is a body rate to Euler rate transformation matrix. Table 
2 gives the state variables for the navigation filter. The twelve state variables include the 
outputs of the three integrator blocks, the estimated current in north and east direction 


components, and the bias estimates for the angular rate readings. (Bachmann 96) 








TABLE 2: State Variables of the Kalman Filter (Bachmann 96) 







Ten of the state components are “continuous time”: the three Euler angles (0,6, y ), 
two horizontal velocities (x,, y,), two horizontal positions (x, ,y,), and three angular rate 


bias estimates. Continuous time integration is approximated by numerical integration, 
making these “continuous time” components discrete time values in the reality of the digital 
filter. This is necessary due to the minimum integration sampling time limitation of the 


computer and A/D hardware. The apparent ocean current values (x,,,) are updated 


aperiodically as a result of both diving and wave action, which produce inherently discrete 
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gps fix information. This discrete event dynamic system is well suited to application of 
Kalman filter theory to obtain optimal time-varying values for the gain matrices K; in 
Figure 9. However, at the time of writing this thesis, there are inadequate statistics on 
DGPS noise and AUV maneuvering as needed by this approach. Therefore, bandwidth 
and steady-state error considerations were used to compute initial constant gains 
(Bachmann 95, McGhee 95), which were subsequently adjusted based on the results of 
experimental studies. (Bachmann 96) 


One area for future project work involves obtaining the necessary statistical data 
needed for refinement of the aperiodic, gps update portion of the filter. The optimum reset 
weight for application to the final integrator block could then be determined. Additionally, 
application of the gps fix interval (1/At) just prior to K, is under consideration for removal. 

The principal difference between the current filter and that described in Bachmann 
(95) regards the point in the filter process at which the apparent current error correction iS 
made. The previous filter added the apparent current to the water speed. The difference 
between this value and the estimated north and east velocities was input to the north and 
east accelerations with a gain K,. Poor initial sea test results in Bachmann (95) indicated 


this approach was possibly underdamped or even unstable. The present approach is to 
apply the apparent current as feedback to the output of the third integrator block, prior to 
input to the final, position integrator. (Bachmann 96) 

The continuous state portion of Figure 9 shows that the Euler angle and linear velocity 
outputs are fed back to the corresponding integrator inputs. Thus, with diagonal gain 


matrices K,, K,, and K;, each of these integrators is in fact a low pass filter for its 


respective inputs (Bachmann 96). Figure 10 isolates one feedback loop to help illustrate 
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this relationship. The integrator block is shown using s-domain (Laplace transform) 
Enon: This approach prevents unlimited state estimate growth caused by unmodeled 
bias errors in state derivative inputs to the integrators (Bachmann 96). Complementary, 
low frequency information from an independent source (accelerometers) 1s also furnished 
to each inieieiiae to correct for long-term decay of the state estimates resulting from this 


feedback (McGhee 95). The low frequency information sources include attitude estimates 


from accelerations sensed by the accelerometers (%,,¥,,%,), the magnetic compass 
readings (‘¥, ), and water speed (u,,). (Bachmann 96) 
angle estimation 


from 
accelerometer i 









estimated 
angular Euler 
rate sensor angle 


Figure 10: Complementary Filter Feedback Loop for Euler Angle Estimation 


The IMU acceleration readings require correction in addition to filtering. The 
accelerometer data is utilized as an inclinometer, to determine how much of the specific 
force felt along each axis is due to gravity. Computed gravity is then subtracted from 
specific force readings of the accelerometers (i, j, 7), to transform them into 


accelerations, prior to rotation into earth-fixed coordinate values (%, ,¥, ,7,). (Bachmann 


96) 
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The rate sensor input in Figure 10 is added to accelerometer attitude estimates after the 
gain matrix is applied. This signal already has the estimated bias removed utilizing the low 
pass filter methodology derived in Chapter II and resulting in Equation 2.7. New biases are 
calculated on each filter cycle by the calculateBiasCorrections function of the insClass, 
and are aoplied to new navigational state information in the applyBiasCorrections function. 
Filter response to example and real world inputs will be discussed in detail in Chapter VI, 


System Testing. 


C. IMPLEMENTATION DESCRIPTION 


Figure 11 shows the revised data flow between software objects. The tasks performed 
by the SANS software can be divided into two basic categories. The primary tasks are 
related sorediculating the current position and other navigational state information. This 
includes processing incoming GPS data, IMU data, water-speed, and heading information, 
and integrating all of this information through the navigational filter to obtain a fix. These 
tasks are performed by the gpsClass, insClass, and Navigator software objects respectively. 
The secondary tasks involve hardware interfacing, communications, data filtering and unit 
conversion. These basic but crucial tasks are handled by the Sampler, Buffer, compBuffer, 
gpsBuffer, A2D, Serial Port, compassPort, and gpsPort software objects. The main 
program serves to drive the other objects by continually querying the navigator for position 
updates and performs output to the user screen and data files from a single location. Real 
time navigation source code is provided in Appendix A. Supporting serial communication 


and other administrative function code is provided in Appendix B. The following summary 
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Figure 11: SANS Data Flow Between Software Objects 











of the source code is presented bottom-up to illustrate construction of the navigation state 


from the individual data elements. (Bachmann 95) 


1. Compass Data 


The compassClass contains the code and member objects which implement reception 
of compass messages in a design similar to, with the exception of specific hardware details, 
the gpsClass. Private member compassPort instantiates a kind of serialPortClass object to 
allow data communication on COM2. CompassPort in turn has private member 
compBufferClass which provides a kind of bufferClass structure for temporary storage of 
incoming compass messages. Figure 12 illustrates the compBufferClass and 
gpsBufferClass data structures. The compassClass therefore contains code to 
communicate with the serial port, as well as to check the “checksum” and header of each 
compass message received. The samplerClass object instantiates compassClass object 
“compl” and periodically interrogates compl to empty the buffer of information. 


(Bachmann 95, Walker 96) 


2. GPS Data 


The gpsClass, as previously mentioned, is similar in design to the compassClass, with 
differences driven by the different message formats, and it utilizes COM1. It obtains GPS 
position messages in the Motorola proprietary format (@@Ea). Before the code 
recognizes a GPS message as being valid, the message must pass three conditions; 1) it 
must have a valid checksum, 2) the fix must be based on at least 4 satellites, and 3) the 


differential bit in the message must be set (ie., the fix must have the differential correction 
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applied to it). The navigatorClass instantiates gpsClass object “gps1” and interrogates gps1 


to empty its buffer directly. (Bachmann 95, Walker 96) 
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Figure 12: Buffer Data Structures 


3. Inertial Sensor Data 


Inertial sensor data passes through the new filter circuit board. From there, it is input 


directly to the A/D converter module in the processor. 


a. A/D 


The A/D module came with demonstration C source code provided by the unit 
manufacturer. Walker (96) modified the demo code and converted it to C++ for the SANS 
application. The a2dClass provides all of the requisite software operation for the A/D 
module in the E.S.P. computer, which is completely controlled through software. Control 


is maintained through the manipulation of the A/D Control Register and the A/D Status 
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Register. These registers are manipulated by writing to and reading from specific memory 
addresses. The a2dClass is designed with some degree of user flexibility. For instance, the 
user can choose between one of two base addresses. (Walker 96) 

- The SANS software only uses a few of the member functions in accomplishing 
its missin Those member functions not directly utilized in this particular application are 
useful for troubleshooting, or allow a variety of options for specific applications. The 
following general discussion explains how the A/D module works in the SANS application. 


(Walker 96) 


The A/D provides 12 bits of resolution, or 2” = 4096 discrete quantization 
levels. The A/D module may be employed in differential mode or single-ended mode. The 
SANS application employs the A2D in the single-ended mode of operation. The A2D 
samples the dual-ended swing of the IMU sensor signals, and represents these voltages as 
a digital value in the range 0 - 4095. A general A/D conversion table is provided as Table 
3 to further illustrate how the sensor voltages are mapped over to their digital equivalents. 


(Walker 96) 


TABLE 3: A2D DC-to-Digital Conversion Mapping (Walker 96) 
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When an a2dClass object is instantiated, the class constructor sets several default 
data faeniber values, and then reads the A/D configuration file A2D.cfg. This 
configuration file provides simple user update of A/D module operation without 
recompiling the source code. The constructor initializes the system addresses, then 
snitializes the A/D hardware using those variables that were loaded upon reading the 
configuration file. The a2dClass object is instantiated by the samplerClass object as 
“42d1’. It is a private data member of the samplerClass. (Walker 96) 

The A/D module is set into operation by a call to the samplerClass function 
initSampler(). It utilizes a2dClass member functions to program the sequencer and tell it 
which channels to sample and in what order, resets the A/D First-In-First-Out (FIFO) to 
enable it to receive data, and then toggles the trigger bit in the A/D Control Register from 


a low to a high, which starts the A/D into operation. (Walker 96) 


4. Sampler 


The samplerClass object prepares raw IMU, heading, and water speed data for use by 
the INS. This preparation includes simple filtering, unit conversion, and time stamping. 
Figure 13 provides a summary of the principal class members and functions, with 
psuedocode descriptions of the principal methods. The samplerClass interface consists of 
a single method (getSample) which controls the data formatting and returns a formatted 
sample if valid raw data is available, and a negative response otherwise. (Bachmann 95) 

Figure 14 provides an illustration of the process of obtaining samples from the A/D. 
During SANS operation, the samplerClass member function readSamples() is called 


repeatedly to retrieve inertial data from the A/D FIFO. It first checks to ensure that the 
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Figure 13: samplerClass Summary 
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FIFO is not FULL. If the FIFO ever gets filled without being immediately emptied, data 
will continue to push into the FIFO. There is no room for this additional data and all 
information from that point on will be lost. Preventing the FIFO from overflowing is 
critical for proper SANS operation. If this check is ever true, the SANS software has been 
cewiten to reinitialize the a2d and continue to navigate. One full FIFO plus the data 
received in the time since the overflow will be discarded. This will result in a very short 
period of lost data with a minimal impact on navigation accuracy. 

To prevent FIFO overflow, one need only be mindful of the rate at which the A/D is 
sampling its inputs and be sure the A/D FIFO is emptied at the same rate or faster. If the 
FIFO does have data in it, this data is emptied from the FIFO and te in a doubly- 
subscripted array with 8 rows and 1000 columns to coincide with storing up to 1000, 8 
channel samples of sensor data. This type of data structure is used to temporarily store the 
data to enable access to a history of samples. Figure 15 presents a model of this array. | 


(Walker 96) 





0 [ aa hee cee eee ee ee we 
Sample Number/Array Index 
Figure 15: Model of the A2D Sample Array (Walker 96) 
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The first action taken by the Sampler when a packet is received is to time stamp it. 
Since the time difference between the eight samples contained in a single message packet 
is relatively aia the Sampler object then respectively averages the eight corresponding 
data variables contained in a packet. As the samples are emptied from the FIFO, the 
yacisbie immeCountee” is incremented once for every 8 samples. This variable is then 
multiplied by the sample period to calculate the “deltaT”, or the time between adjacent 
samples. The samplerClass code then averages over all the samples received since the last 
sample was taken from this array. The averaged measurements which result represent a 
simple low-pass filtering of the samples. This has the effect of filtering out small 
fluctuations in the data. (Bachmann 95, Walker 96) | 

The integers contained in a sample are digital measurements of analog voltages output 
by the SANS sensors. Once these eight filtered measurements are obtained they are 
converted from voltages to units which are usable by the INS object (.e., feet and radians). 
Finally, each of the measurements is checked to ensure that it is within the limits of the 
seals from which it came. If any values fall outside the capabilities of the sensor from 


which it came, the entire packet is considered invalid and discarded. (Bachmann 95) 


5. INS 


The INS class implements the SANS inertial navigation. It is the most complex class 
in the software. It has been changed very little as the project has evolved. Figure 16 
provides a summary of the principal member objects and functions which constitute the 
INS methods. The interface consists of three public methods. Each is directly involved in 


the implementation of the twelve-state Kalman filter. The primary method (insPosition) 
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private member 









insClass 










Constructor 
-- read config file (K values, T) 
-- velocities[O]--> [5] =0.0 ( %9,46,6,W) 


-- posture[0]-->[5] = 0.0 (x, y, Zz, 0, 0, y 
-- current{0] --> [2] = 0.0 






| 

| insSetUp 

| .- set initial true heading, 
: speed 
| 

| 

| 





- init current to 0.0 
- lastGPStime = 
originTime 





getSample() initSampler()}_ | calculateBiasCorrection() 


insPosition correctPosition 
- getSample(newSample) - correct for new day if nec. 
-applyBiasCorrections(newSample) - deltaT = positTime - lastGPStime 
- transformBodyRates () (to euler rates) - calculate INS error 
- calculate estimated pitch/roll/yaw rates | - Reinit posture to gps fix 


- integrate estimated angular rate to - Add gain filtered error to to 
obtain angles previous errors 


- transform accels and water speedto | - update time 
earth coordinates 


-calculate, apply waterSpeed Correction Admin Functions 
2h | | - transformAccels (body to earth, 
- integrate accels to obtain velocities eliminate g from z) 


- integrate velocities to obtain posture - transformWaterSpeed(earth coord) 


- save estimated positions -transformBodyRates(body to earth, 
| Euler) 


- buildBodyRateMatrix 

- buildRotationMatrnix 

- postimultiplication operator 
- readInsConfig 


Figure 16: insClass Summary 
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combines all sensor information and uses the Kalman filter to produce a dead reckoning 
position estimate. The other methods support the primary method by performing one-time 
or periodic operations. Initialization of the INS is performed by the insSetUp method, 
which sets the INS posture at the grid coordinate origin, sets an initial heading and speed, 
and ides the beginning of the first integration intervals. The last public method of the 
class (correctPosition) inputs GPS information to reinitialize the INS position while 
determining a current and error correction bias. The INS class instantiates a samplerClass 


object “sam1”, from which it obtains all sensor data except for GPS position fixes. 


(Bachmann 95) 


6. Navigator 


The navigatorClass acts as coordinator of all navigational information. As such it 
determines which source is currently providing the best information, converts various 
position formats from one format to another, and instantiates the GPS and INS objects 
“gps” and “ins1”. Like the insClass, this portion of the code has been changed very little 
as the project has evolved. Figure 17 provides a summary of the class members and 
functions that provide the principal navigation methods. The interface to the object is made 
up of two public methods. (Bachmann 95) 

The main program instantiates navigatorClass object “nav1”. The first method of the 
navigatorClass (initializeNavigator), initializes nav1, preparing it to begin providing the 
current position upon request. This method obtains an initial GPS fix for use as the origin 
of the grid used by the INS object to specify positions, and calls the initialization method 


of the INS. (Bachmann 95) 
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Main 
-- instantiates “navl’’ 


-- initializes navigator 


-- prints initial posit, 


screen set up 
-- continuous loop: 
while(T) fix received 


= navl.navPosit(curLoc) 
if so, print and file 


initializeNav() 


-- loops for gps fix, calling 
gpsPosition() 


-- saves initial fix as the origin 


-- passes origin time to 
insl.insSetUp() 





insSetUp() 


initSampler() getSample() 


calculateBiasCorrections()| | initCompass() 


main - ‘curLoc’ 
ins & sam - ‘newSample’ 


gps/ins flags 
latLongPosition ‘navLatLong’ 
grid ‘est’ (ins est. posit.) 
GPSdata ‘satPosition’ 
float ne 
[0] 








stampedSample (nav structure) 


[3] phi (roll) 


[4] theta (pitch) 


[5] psi (yaw) 


[6]. water speed 


[7] heading 


double sample[11] (sampler converted) 
double deltaT 
float bias[3] 
float current[3] 


navPosit() 
-- get gps, ins fixes 
both? 


gps posit--> 
ins L.correctPosition() 


ins only? 
update ins posit 


gps only? 


gps posit--> 
ins1.correctPosition() 
new ins posit = gps posit 





Figure 17: Navigation Class and Initialization Summary 





nav - ‘posit’ 






(pre-sampler) 
x acceleration 
[1] y acceleration 
[2] zacceleration 














The second navigator method (navPosit) drives both the GPS and INS objects, and 
provides the navigator’s best estimate of current position in hours, minutes, seconds and 
milliseconds of latitude and longitude. Each time the method is invoked, it interfaces with 
the GPS and INS objects to determine if none, one, or both have an updated estimate of the 
current position. If no update is available, the navigator returns a negative reply indicating 
that it can not provide a position update. If only INS information is available, it is returned 
as the current estimated position. Whenever GPS information is available, it overrides the 
INS estimate of position. GPS information is also passed to the INS object as a reference 
for reinitialization and error estimation purposes. (Bachmann 95) 

The navigator deals with three different position formats. GPS ee from the 
Motorola receiver are initially obtained entirely as latitude/longitude in milliseconds. INS 
positions are expressed in x-y grid coordinates based upon a navigator-stored origin. GPS 
positions must be converted to grid coordinates prior to utilization by the INS. The 
positions produced by the navigator are expressed in hours, minutes and seconds of latitude 
and insti A total of four methods are used to convert from one format to another. 


Figure 18 illustrates uses and conversions of the different position formats. (Bachmann 95) 


re Communication Objects 


The bufferClass and serialPortClass objects are abstract parent classes from which 
specific instances are instantiated for the compassClass and gpsClass, respectively. As 
such, they contain the common class members and functions to support the routine but 


essential tasks of serial port communication and buffering received data. 
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USER 








Positions expressed in 
hours, minutes, seconds 
and milliseconds of 
latitude and longitude. 


NAVIGATOR 


Positions expressed in 
milliseconds of latitude 
and longitude 


Positions expressed 
in grid coordinates 


Figure 18: Navigation Position Format Utilization (Bachmann 95) 


D. SUMMARY 


The SANS software is designed to produce continuously accurate navigational 
information in real time. While submerged, IMU, heading and water-speed information are 
processed by the SANS Inertial Navigation System (INS) to produce a dead reckoning 
position estimation. This is integrated with DGPS information obtained during aperiodic 
surfacings using Kalman filtering techniques. The DGPS information is sa to reset the 
position of the INS. It is also used to generate an apparent current vector to correct future 


INS position estimates. (Bachmann 95) 
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The software was implemented using object oriented paradigms. It was written in 
Banand version 3.1, C** for use on an IBM-compatible processor. The primary tasks of the 
software are estimation of current position and communication. The former is handled by 
the Navigator, Sampler, a2d, Compass, INS, and GPS classes. The later is accomplished by 
the bufferClass, compBufferClass, gpsBufferClass, serialPortClass, compassPortClass, 

and gpsPortClass objects. (Bachmann 95) 
The next chapter of this thesis will present the testing methodology and results for the 


tilt-table tests of the operational code. 
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V. SYSTEM TESTING 


A. INTRODUCTION 


This chapter presents both the testing methodology and the experimental results of the 
tilt-table testing used to determine the functionality and accuracy of the SANS attitude 
estimation. These tests focus on the operational C++ code, on determination of optimal 
gain settings for the attitude portion of the navigation filter, and on evaluation of the 
hardware accuracy and noise characteristics in a controlled environment. Factors which 


control attitude response include the K, gain value, the bias weight (biasWght), sample 


weight (sampleW ght), and the x and y axis accelerometer scale factors. 

_ As areminder from Chapter II, the rate sensor input in Figure 11 has the estimated bias 
removed utilizing the low pass filter methodology resulting in Equation 2.7. Further 
background on low pass filter bias response is provided below in order to show the 


reasoning behind the testing methodology and to help explain the results. 


B. LOW PASS FILTER BIAS RESPONSE 


Applying Mason’s formula to the signal-flow graph of Figure 2 from Chapter 2, in the 
s (Laplace transform) domain gives the transfer function of a low pass filter as 


(Eq 5.1) 


15 1 U(S)acral _ Lfoutput} _ Y(s) 
Pets; bts UGS) aan L{input } U(s) 
A typical tilt-table test of the attitude and angular rate sensors involves a step input of 


a constant roll rate to a commanded roll angle, for example, 10 degrees per second to an 
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angle of 45 degrees, resulting in a 4.5 second input. The filter bias estimation response can 


be determined from x(t) = u(t), leading, in the s domain, to X(s) = U(s) =1/s, and 


i J 
= _ ] a 
Y(s) = G(s)X(S) = Ss a s(1 + TS) = s(s+1) (Eq 5.2) 
1+ts 1 
Thus 
wi af 
y(t) = tft. ; =-1-e' (Eq 5.3) 
and 
| | 
y(t) = = (Bq 5.4) 


which represents the bias filter output slope. Thus, the simplified response of the bias 
estimation to the initial roll input is an exponential rise beginning at the instant the input is 
initiated. After one time constant (Tt), 63 percent of the input value has been reached. The 
output value gradually approaches the limit of the input as time continues. This is 


graphically represented in Figure 19. 







10.0 
output 


6.3 (since 1/e ~ .37) 






20 20 +T 


time 
Figure 19: Bias Filter Response to a Roll Rate Step Input of 10°/sec 
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The developed testing procedure for the SANS allows approximately 20 seconds of 
initial stabilization time for the components and filter to “steady out”. This was followed 
by an initial roll input, a similar stabilization period after the platform had reached the 
commanded angle, and then return to the zero position at the same rate. Typically, two of 
these cycles were performed under each testing condition. 


To shift a unit step to start at 20 seconds 


0 for t< 20 
y(t) = _(t-20) (Eq 5.5) 
l-e ° for t >20 


The example input pulse of 4.5 seconds can be written 





x(t) = 10(u(t— 20) — u(t — 24.5)) | (Eq 5.6) 
giving 
0 for t< 20 
1-20 
ise * for 20<t<24.5 
y(t) = (Eq 5.7) 
| —t—20 _t-245 
10 tne Tt } v for DA. 1 
Since 
2 
e = Ltxt at... (Eq 5.8) 


then, for small x 


(Eq 5.9) 


t —20 t —20 
om wie es I eee <ft< 
y(t) 10/1 (1 —~]| 10 aa | for  20<t<245 
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for a time constant of 1000 seconds. This produces the first part of the response illustrated 


in Figure 20. 
phe Bc 
10 Slope = + = 999 7-0! 
05 wm ee ee 
20 24.5 | time 


Figure 20: Estimated Short Term Bias Response to a 45 Roll Completed in 4.5 
Seconds 


For times equal to or greater than 24.5, 
(Eq 5.10) 


t— 24.5 t —24.5 
t—-20 t-—24.5 os 1000 am 1000 
y(t) = 10 1000 a 1000 = 0.045e 





This result is shown in Figure 21 on a longer scale to illustrate the gradual correction 


over time. 


0.1 


05 —---—----—----—--- -- -- - ee ee ee 





20 24.5 =1000 


Figure 21: Estimated Long Term Bias Response to a 45° Roll Completed in 4.5 
Seconds 


Taken together, Figures 20 and 21 illustrate that the bias response of a low pass filter 


to a time-shifted step roll input is a rapid rise to the calculated value, followed over the 
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length of the relaxation time constant by a gradual correction to zero. Combining this 
response with the complementary filter design, incorporated as depicted in Figure 11, 
results in the time domain filter response including a time lag effect which barely sees 
minor transients. The initial response to a step change in attitude comes almost entirely 
from fie angular rate sensors. Over time, input from the accelerometers takes over and 
compensates exactly for the decay of the rate sensors. The nature of this response 
influenced development of the testing methodology and is directly reflected in the 


following testing results. 


C. FILTER TESTING METHODOLOGY 
The tilt-table testing methodology has evolved through Bachmann (95) and Walker 


(96). Although basically unchanged from the method used in Walker (96), it is presented 
here in a standardized, sequential order with extensive background for the first time. It is 
also presented at Appendix D in a checklist format. The testing methodology is designed 
to separate the complementary effects of the filter and treat them individually before 
evaluating the entire filter process. 

The SANS is mounted to the tilt-table described in (Bachmann 96) for a series of pitch 
and roll tests. If the unit is carefully leveled prior to testing, the actual commanded attitudes 
are extremely accurate in reference to real-world pitch and roll angles. Relative angle 
excursions are always extremely accurate on the tilt-table. The amount of the actual angle 
excursion is the important value for the testing. In other words, a valid 45 degree pitch from 
a beginning baseline of 2 degrees to 47 degrees, for example, is a successful test for the 


IMU. Once calibrated and installed in Phoenix, the SANS becomes the reference for 
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attitude determination. That is, if roll and pitch SANS outputs are zero, then this defines 





level orientation for Phoenix. 

The general procedure is to allow a 15 to 20 second period for the sensors to initialize 
and stabilize after the filter code begins execution. This is followed by a pitch or roll 
eecusion to 45 degrees at various commanded rates (consistent during each individual 
run). The unit is then tilted back to the zero position, followed by a roll excursion in the 
opposite direction, and then finally back again to zero, Each movement is followed by the 
stabilization period to allow observation of filter effects. Those cases where the excursions 
were both in the same direction reflected physical limitations as to how the SANS box 
could be mounted on the tilt-table. Maximum tilt rate was 90 degrees per second, but tests 
were normally conducted at either 10 or 45 degrees per second. These conditions are much 
more severe than those encountered by the SANS in the real world, with the possible 
exception of surfaced operations in a very heavy sea state, and therefore represent worst 
case performance for the filter. 


In order to determine the rate sensor bias value, K, is set to zero to prevent 


accelerometer inputs from effecting the results. Therefore, only the high frequency angular 
rate and bias get to the first integrator. Any errors in attitude can then be attributed to the 
bias and scale factor. The appropriate initial angular rate scale factor (qScale for pitch, etc.) 
is then determined by taking the commanded tilt-table angles as truth. The scale factor 
adjusts the output of the IMU to the actual tilt results. Starting with a baseline of 1.0, it is 


possible to continuously apply the ratio of indicated and actual angles to the current setting 


in order to scale it to a proper value. For example, if the SANS says the unit pitched to 41° 








when the actual pitch was 45°, the new scale factor is increased to 45/41 multiplied by the 
old scale factor. 

The initial’ bias weight (biasWght) is chosen through a combination of project 
experience and filter theory considerations. Extensive simulation and _ tilt-table 
eseaiente can then refine the proper values prior to at-sea testing. 

After setting the gain weight to some value other than zero, multiple test runs can 
refine the proper settings. The accelerometer scale factors are then adjusted in the same 
manner as the angular rate scale factors if indications show that the combined inputs result 
in inaccurate angle excursions. A complete tuning of one axis may take an extensive set of 


alternating adjustments to the various factors, as illustrated in the testing results provided. 


D. IMU TEST RESULTS 


The testing results included here utilized the current hardware configuration, along 
with the original code from Bachmann (95) only slightly modified to improve input/output 
rates. This resulted in update rates of approximately 18 Hz. The complete code revision 
described in this thesis resulted in an increased update rate of 40 Hz, making the filter real- 
time capable for the first time. That update rate unfortunately overwhelms the internal data 
storage of the SANS in the current configuration, so further testing will have to either be 
done at reduced rates or be conducted after new, larger storage cards (now available) have 
been obtained. 


Figure 22 shows the initial pitch test run. Both K, values are set to 0.0, isolating the 


angle-rate input from the accelerometer input. Pitch was at arate of 10 degrees per second. 


65 








The qScale value had already been adjusted to 4.02 to reflect 45° of pitch. When compared 
to previous project results (see Walker (96) and Bachmann (95)), the faster update rate 
significantly reduced initial overshoot of the final pitch angles. The stabilization periods 
following each pitch show that the effects of the filter cancel in that the initial slight 
iii gradually return to the proper value, regardless of pitch direction, as expected 
from the earlier explanation. In fact, for the pitch which is initiated at approximately 20 
seconds, if no other pitch excursions occurred, the angle value would become essentially 
45° by 1020 seconds (20 +t). The stabilization period is only a small fraction of the time 
constant, and the bias is subtracted from each new sample. Thus, the accumulated bias 
from the excursion is only partially corrected for, with a slope in the direction of the 
“correct” value. 

Figure 23 shows a second pitch test with all values unchanged with the exception of T, 
which increased from 1000 to 5000. Ideally, the filter should be initialized for a period of 
one time constant, however, the shorter stabilization periods here are sufficient to 
demonstrate filter behavior. The stabilization periods of Figure 23 show a flatter slope than 
those of Figure 22. This reduced slope shows that increasing T minimizes the accumulated 
rate bias. 

Turning to the roll axis, Figure 24 shows the initial roll test. The time constant T has 
been reset to 1000 seconds. The roll rate is still 10 degrees per second and the initial scale 
factor (pScale) was set to be 4.01. The nearly identical scale factors show the IMU to be 


very consistent between axis. Otherwise, the roll results are similar to the pitch results. 
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Figure 23: Pitch Test, K, = 0.0, t = 5000, qScale = 4.02, 10°/sec 
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0 10 20 Zs«O 50 60 70 80 90 
Figure 24: Initial Roll Test, K, = 0.0, t = 1000, pScale = 4.01, 10°/sec 


The roll rate was increased to 40 degrees per second for the second roll test. This 
becomes obvious with the more widely separated fix dots on the graph in Figure 25. Since 
the update may occur at any point during it’s cycle (worst case immediately before the 
commanded angle is reached), more overshoot is possible, and in fact occurs. This leads 
to a more pronounced return effect during the stabilization periods. 

The third roll test, shown at Figure 26, has identical settings to the previous test with 
the exception of K,, which has been set to 0.01 to allow an accelerometer effect to return. 
This is what causes the wander in roll angle seen in the initial stabilization period. This 
effect is also present in the stabilization following the initial roll, but is less pronounced 


after the return to the zero position as the time grows closer to the initial time constant. 
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0 10 20 0 40 50 60 70 80 90 
Figure 25: Roll Test: <x, = 0.0, t= 1000, pScale = 4.01, 40 °/sec 


"rollS.tst” x. 





0 20 40 60 80 100 120 140 160 


Figure 26: Roll Test: Kk, = 0.01, t= 1000, pScale = 4.01, 40 °/ sec 
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Increasing K, to 0.05 and reducing t to 200 produces the results of Figure 27. These 
stabilization periods are characterized by more aggressive corrections to the “proper” 
angle. Both Figure 26 and 27 show the importance of increasing the filter update rate from 
the 18 Hz rate shown to the 40 Hz rate achieved in this thesis to prevent undershoot and 
overshoot due to sampling effects. The results of Figure 27 are essentially duplicated, 


although at a reduced roll rate of 10 degrees per second, in Figure 28. 





0 20 40 60 80 100 120 
Figure 27: Roll Test: K, = 0.05, t = 200, pScale = 4.01, 40 “/sec 


The following roll test, Figure 29, shows the effect of varying the accelerometer scale 


factor (yAccelScale) from 1.34 to 1.405. The stabilization periods are flatter with respect 
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to the “correct” angle and the accelerometer effects less pronounced when compared to 
Figure 28. 

Figure 30 returns the time constant to 1000 seconds while also doubling K,. It was 
determined at this point that the yAccelScale value had been adjusted too high. Prior to the 
next roll test (Figure 31), it was adjusted by (45/48) * 1.405 since the unit computed an 
initial roll of 48° vice 45. Figure 31 shows a flatter response, but there is still some 
overshoot. The pScale was adjusted again for the test shown in Figure 32 by the amount 
(45/46) * 4.01. Finally, the yAccelScale was adjusted once again by (45/44) * 1.317 to 


produce the output in Figure 33. This sequence clearly illustrates the alternating, gradually 
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Figure 30: Roll Test: K, = 0.1, t = 1000, 10°/sec, yAccelScale = 1.405 
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finer tuning approach which must be taken in order to tune a filter of this complexity. 
Figure 33 clearly has the least overshoot/undershoot and the flattest stabilization periods 
while exhibiting a proper correction tendency before the next input 1s encountered. 
Figures 34 and 35 are provided to illustrate filter response at the more radical rates of 
45°/sec si 90°/sec. Although there is slightly more overshoot, as expected, even at these 
extremes, the filter behaves predictably and well within acceptable accuracy for the 


Phoenix or other small scale portable navigation applications. 
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Figure 33: Roll Test: K, = 0.1, t = 1000, yAccelScale = 1.347, 10°/sec 
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Figure 35: Roll Test: same as previous, with 90°/sec vice 45 
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E. SUMMARY 


This chapter has provided a methodology for dynamic tilt-table testing with rationale 
and illustrative experimental results. Taken together, the results graphically show that the 
SANS design, code architecture, and filter implementation are performing as expected. 
Additionally, while room for some improvement remains, the sensor/filter combination is 
easily accurate enough to meet both the Phoenix AUV and other potential small scale 
portable navigation applications. It is important in reviewing the results presented to 
remember that these testing conditions are much more severe than are likely to be 
encountered in actual SANS operation except when surfaced in significant sea states. 
Other independent testing of the SANS approach (Henault 96) suggests that attitude 
estimation to an accuracy of a few tenths of degrees should be realized in normal operating 
conditions. 

Addition of a math coprocessor to the E.S.P CPU module has increased performance 
dramatically and decreased the undersampling seen, as expected by Walker (96). 
Accompanying code revisions have resulted in a legitimate real-time navigation filter 
which is expected to improve accuracy even further. The final chapter of this thesis will 


review conclusions reached and recommendations for future project work. 
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VI. CONCLUSIONS AND wi aca FOR FUTURE 


A. CONCLUSIONS 


The research topics addressed by this thesis were: 1) evaluate the hardware and 
software architecture of the SANS, 2) develop a calibration procedure for the SANS 
navigation filter, 3) evaluate the scsi performance of the SANS navigation filter, and 4) 
evaluate the SANS hardware and software architecture for installation in the Phoenix AUV. 
Each incremental step in the SANS project work has provided evolutionary improvement 
in capability and performance. Walker (96) built on the Bachmann (95) hardware 
prototype and provided the current hardware capability. This thesis has improved on the 
code architecture of Bachmann (95) to accommodate the greatly increased processing 
speeds resulting from the Walker (96) hardware configuration and addition of a math 
coprocessor. 

A basic tilt-table testing methodology was utilized for an overall evaluation of the 
SANS attitude estimation pursuant to addressing the research issues. Combining the 
procedures used in Walker (96) and Bachmann (95) to produce a specific filter calibration 
procedure simultaneously addressed all of the topics in a general manner. The results 
showed that the filter is working correctly and as expected from the supporting theory. 
Furthermore, the real-time capability now makes SANS a bonafide option as a new 
navigation solution for Phoenix or alternative small scale portable navigation applications. 


The SANS project is now poised for meaningful at-sea trials to further validate the recent 
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improvements to allow further development of the linear velocity and position estimation 


portions of the filter. 


B. RECOMMENDATIONS FOR FUTURE WORK 


There remain many areas for further research on the SANS project. The next major 
step will be at-sea testing utilizing a tow fish as in Bachmann (95). Successful completion 
of these tests will make the SANS ready for adaptation and installation in Phoenix if it is 
chosen as the navigation solution. Incorporation into Phoenix is expected to be very 
straightforward. The ethernet connection can be utilized to pass the Phoenix “Officer of 
the Deck” software module the required navigation state elements. These elements are 
currently stored at each update and written to a data file. Compatibility issues should be 
limited to data communication between SANS and the Phoenix navigator software. In the 
meantime, purchase of a larger PCMCIA SRAM card will immediately alleviate the data 
storage problem encountered during laboratory testing resulting from the faster processing 
speeds. 

Consideration should be given to updating the software utilized in SANS. Two 
approaches exist. The first is to update the DOS/BORLAND PC environment by 
upgrading to the latest versions. This option will entail rewriting some of the basic input/ 
output system function calls. The second option would be a complete rewrite to make the 
software compatible with the final Linux or LonWorks implementation option that is 
incorporated into Phoenix. Although more involved, this option is attractive because it 


prevents a proliferation of different operating systems within the same architecture. 
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Postprocessing of the navigation data file remains an unfinished area from Bachmann 
(95) and Walker (96). Test runs could be repeated multiple times to more easily optimize 
the Kalman filter gains. In a related matter, the incorporation of the aperiodic GPS updates 
into the overall Kalman filter scheme also still remains to be refined. The author hopes that 


the results presented in this thesis will prove to be valuable in this ongoing effort. 
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APPENDIX A: 





Real Time Navigation Source Code (C++) 


A. TOWTYPES.H 


#ifndef 


#define 


#include "globals.h" 


#define 
#define 
#define 


#define 
#define 


#define 


typedef 
typedef 
typedef 


typedef 
typedef 
typedef 


struct latLongMilSec { 


_. TOETY PES_H 
__TOETYPES_H 


// Types used by serial communications software 
GPSBLOCKSIZE 76 


PACKETSIZE 133 
COMPSIZE 60 


// Size of Motorola @@Ea position message 
// Size of packet received via X-modem protocol 


ONE_G 32.2185 
GRAVITY 32.2185 


// One g in feet per second 
// In feet per second 
TicksToSecs(x) ((double) ((10 * x) / 182)) 
char ONEBYTE; 

short TWOBYTE; 

long FOURBYTE; 


unsigned char UNSIGNED_ONEBYTE; 
unsigned short UNSIGNED_TWOBYTE; 
unsigned long UNSIGNED_FOURBYTE; 


// Holds lat/long expressed in miliseconds 


long latitude; 
long longitude; 


a 


// Holds a latitude or longitude expressed in hours minutes and degrees 
struct T_GEODETIC { 


TWOBYTE 


degrees; 


UNSIGNED _TWOBYTE minutes; 


double 


14 


seconds; 


// Holds a latitude and longitude expressed as T_GEODETICs 
struct latLongPosition { 

T GEODETIC latitude; 

T GEODETIC longitude; 


iar 


Struct orid’-t 


// Holds a grid position 


double X,V,Z; 


a 


struct matrix { 


// 3 X¥ 3 matrix 


float element[3] [3]; 


ee 
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struct vector { : : // 3 X¥ 1 matrix or vector 
float element [3]; 


ie 


// Oversize area to hold a GPS message 
typedef BYTE GPSdata[2 * GPSBLOCKSIZE]; 





// Defines a type for holding compass messages 
typedef. BYTE compData[2 * COMPSIZE]; 


// Structure for passing around various types of INS information. 
// The positions in the sample field of a stampedSample structure 

// sample[0]: x acceleration gnuplot: 2 

// sample{1]: y acceleration 3 

// sample[{2]: z acceleration 4 

// sample[3]: phi (roll) 5 

// sample[4]: theta (pitch) 6 

// sample[5]: psi (yaw) 7 

// sample[6]: water speed 

// sample[7]: heading 


struct stampedSample { 


Boolean. gpsFlag; // True -- GPS fix obtained 
Boolean insFlag; // True -- INS fix obtained 
latLongPosition navLatLong; // posit in hours, mins, secs 
grid est; // position as estimated by the INS 
GPSdata satPosition; // the latest GPS position 
float rawSample[8]; // Original readings for post processing 
double sample[11]; // sampler converted sample 
double deltaT; // delta of the sample 
float bias[3]; // bias corrections 
float current{[3]; // error correction current 

}; 

#endif 
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B. TOEFISH.CPP 


#include <stdlib.h> 
#include <stdio.h> 
#include <string.h> 
#include <iostream.h> 
#include <conio.h> 
#include <dos.h> 
#include <time.h> 


#include "toetypes.h" 
#include "“nav.h" 


extern compassPortClass port2; // so breakhandler can call destructors 
extern gpsPortClass portl; // to insure cleanup on program exit 


int breakHandler (void) ; 

void screenSetUp (void); 

void printPosition (const latLongPositions&) ; 
void positOut (stampedSample& posit); 


// Write an INS packet and its timeStamp to the outPut file 
void writeData(const stampedSample& drPosition, ofstream&) ; 


// Write a GPS message to the outPut file. 
void writeGpsData(const GPSdata& satPosition) ; 


JRELEEAEELERLERREER ELE LENA RLERERE AREA EES EERE SARE AL A ARR nO ee oe ee 


PROGRAM: Main 

AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 

DATE: 11 July 1995, last modified January 1997 

FUNCTION: Drives the navigator and its associated software. Counts 
the positions & displays each to the screen. Exited only 
when control break (Ctrl c) is entered at the keyboard. 

RETURNS: 0 

CALLED BY: none 


CALLS: initializeNavigator (nav.h) 
navPosit (nav.h) 
‘printPosition 
breakHandler 
KR KKK KKK KKK KEKE KKK RK KKK KEK KK KKK KKK KKK KE KKK KKK KEKE KRKEKEKEKERE EKER REREKE RR / 
ne 
main () 
{ 
ctrlbrk (breakHandler) ; // trap all breaks to release com ports 
setecbrk(1); // turn break checking on at all times 
char dataFile[] = "att.dat"; 
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cout << "\nWriting attitude data to " << dataFile << endl; 
// Instantiate the navigator (also private members gpsl & ins1l) 
navigatorClass navl; 


ofstream attitudeData(dataFile) ; 


stampedSample curLoc; // Lat/Long of most recent fix 
Boolean fixReceived (FALSE) ; // True if a new fix was received 
int fixCount (0); // Count of navigation fixes received 
float timeCount(0.0); // Counter for screen output 

cerr << "\nInitializing . . ." << endl; 


navi.initializeNavigator (curLoc) ; 


// Check a2d initialization, channels off if y-accel != ~-32.2 
while (curLoc.sample[2}] <= -33.0 || curLoc.sample[2] >= mS bea) 34 
cerr << "reinitializing for a2d channelization" << endl; 
navl.initializeNavigator (curLoc) ; 
navl.navPosit (curLoc) ; 


CLPScr (3 

gotoxy (1,6); 

cerr << "Initialization Complete!" << endl; 
cout << "Initial Position:" << endl; 


// Print the initial position 

cout << "latitude: " << curLoc.navLatLong.latitude.degrees << 
<< curLoc.navLatLong.latitude.minutes << ':' 
<< curLoc.navLatLong.latitude.seconds << endl; 

cout << "longitude: " << curLoc-navLatLong.longitude.degrees << 
<< curLoc.navLatLong.longitude.minutes << ':' 
<< curLoc.navLatLong.longitude.seconds; 


Boe ct 
°° 


screenSetUp(); 


while (TRUE) { // Attempt to get a fix from the navigator 
fixReceived = navil.navPosit(curLoc); 


if (fixReceived) { // New fix received 
// Save fix info to the data file 
writeData(curLoc, attitudeData); 
// Print info to screen at designated print interval 
fixCount++; 
timeCount += curLoc.deltaT; 
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if (timeCount >=-1.0) { 
gotoxy (9,11); 
cout << fixCount << endl; 
positOut (curLoc) ; 
timeCount = 0.0; 


[LELELEEAAE LAER LESTE AAAS LAE ES RELEASE EAA ESS RD RON SBS eee ee eee ee 


PROGRAM : printPosition 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Displays position to the screen 
RETURNS : void 

CALLED BY: main 

CALLS: none 


See aK RK RK RRR KR KK KKK KR RK KKK KKK KKK KKK KEKE KK KEK KRKEKK KEE KER KKK REKK / 


void printPosition (const latLongPosition& posit) 
f 
gotoxy (11,14); 
cout << posit.latitude.degrees << ':'<< 
posit.latitude.minutes << ':' << posit.latitude.seconds << endl; 


gotoxy (12,15); 
cout << posit.longitude.degrees << ':'<< 
posit.longitude.minutes << ':' << posit.longitude.seconds 


<< endl; 


[REKEELEERAAAEEAERERES ELE LER ARE ELAS EAEA AEN AA EEE LEER EAREREA ES ERAN SR EES 


PROGRAM : breakHandler 


AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995 

FUNCTION: Cleans up com ports upon program exit. 
RETURNS : 0 

CALLED BY: main 

CALLS + compass port and gps port destructors 


KKK KR KK KR KK KKK KE KR KEK KKK KE KKK KEK KK KKK KEK KE ERE ERE RE KKK KERR KEKE KKEKEKEKEREREEKR EK / 


int breakHandler (void) 


{ 


port2.~compassPortClass(); 


porti.~gpsPortClass(); 
return 0; // keep the compiler happy 
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LRELELRALELELLAAE ALAS LAR EE SEBEL LALLA LAL EAL REECE RS REA ee 


PROGRAM: screenSetup 


AUTHOR: Eric Bachmann, Randy Walker 
DATE: 12 May 1996 

FUNCTION: Sets up the output screen 
RETURNS : 0 

CALLED BY: main 

CALLS : none 


he KK RK KK KK KR RK IK KK KR KKK KKK KK KR KK KEK KKK KKK KR KEKE RK EK KR KEK REE KEKE KEK EEE KE / 


void screenSetUp (void) 
{ 

gotoxy (4,11); 

eout <= “Fix. "s 


gotoxy (1,14); 
cout << "Latitude: " << "\nLongitude: "; 


gotoxy (1,17); 
cout << "Roll: " << "\nPitch: "; 


gotoxy (1,25); 
cout << "“deltaT: “; 


int col(45),row(1); 


gotoxy (col, row++); 
cout << "x accel: "; 
gotoxy (col, row++) ; 


cout << "y accel: "; 
gotoxy (col, row++) ; 


cout << "zg accel: "; 
gotoxy (col, row++) ; 
cout << “phi dot: "; 


gotoxy (col, row++); 

cout << "theta dot: "; 
gotoxy (col, row++); 

cout << "psi dot: "; 
gotoxy (col, row++) ; 

cout << "water speed: "; 
gotoxy (col, row++) ; 

colt: << “heading: "3 


col = 45; 
row = 12; 


gotoxy (col, row++); 


CoOut -<< "xs "3 
gotoxy (col, row++) ; 
COuUL. a "ye > 
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gotoxy (col, row++) } 
CGouc mx ‘Foe Ne 
gotoxy (col, row++) ; 
cout << "phi: "; 
gotoxy (col, row++) ; 
cout << "theta: "; 
gotoxy (col, row++); 
cout <<-"ps1:s “> 


gotoxy (45,20); 
cout << "Bias Values"; 


gotoxy (60,20); 
cout << "Current Values"; 


LRERELARA ELLA LES ILLS ERA EERE LEER AAS EEE LEER LAE Se ne me mone eae 


PROGRAM:  positOut 


AUTHOR: Eric Bachmann 

DATE: 21 October 1996 
FUNCTION: Updates the Screen 
RETURNS : 0 ; 
CALLED BY: main 

CALLS: none 


BRK RK RR KR KK KR KR RR RR KR KR KE KEK KERR KKK KEK REE KEK KK REKRKE KKK KEKE KEE KKK KR KEK KEK / 


void positOut (stampedSample& posit) 
{ 


printPosition (posit .navLatLong) ; 


if (posit.gpsFlag) { 
gotoxy (20,11); 
cout << "GPS"; 


} 
else { 
gotoxy (20,11); 
cout << " as 
} 


// Output the bias values 
for(int 5° = 35 -] -<° 67 34+): 4 
gotoxy (45,j3+18); 
cout << posit.bias{[j]; 


} 


// Set output precision and fixed format 
//cout.precision (6); 
//cout.setf(los::fixed) ; 
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// Display linear accelrations and angular rates 
for (j = 0; j < 8; j++) { 

gotoxy (59,j3+1); 

cout << posit.rawSample[j]; 


} 


// Display time delta to the screen. 
gotoxy (9,25); 
cout << posit.deltaT; 


// Display roll and pitch 

gotoxy (8,17); 

cout << (posit.sample[{3] * radToDeg) ; 
gotoxy (8,18) ; 

cout << (posit.sample[4] * radToDeg) ; 


// Display current location and posture 
for (j = 0: j < 6; jt) { 

gotoxy (52,j3+12); 

cout << posit.sample[j]; 


} 


// Display error current values 
for (7 = 0s 3 < 37 35%) 4 
gotoxy (60,3+21); 
cout << posit.current[j]; 


} 


// Output the biases 

for (j = 3; 3 < 6; j++) { 
—gotoxy (45,3+18) ; 
cout << posit.bias[j]J; 





[RR RE KK KK KK RRR KE KK RK RR RR KK KKK KKK KKK EKER KKK KEK EK KKK KKK KKK EKER ER ERK KE EK 


PROGRAM : writeData 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Writes the packet and the time stamp contained in a stamped 
sample to the out put file for post processing. 

RETURNS : void 

CALLED BY: navPosit (nav.cpp) 

CALLS : None 


KK kK KR KK HK KR RIK KEK KKK RK KR KE KKK KEK KEKE RE KKK KEK KEKE KKK KEKE KKK KR RE KK EEKEEKEHE / 
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void writeData(const°>stampedSample& drPosition, ofstream& attitudeData) 


{ 
static float elapsedTime(0.0); 


elapsedTime += drPosition.deltaT; 


// Output attitude data to a file 

attitudeData 
<< elapsedTime << ' '' 
<< drPosition.sample[{0} << ' ' 
<< -1.0 * drPosition.sample[i] << ' ' 
<< drPosition.sample[2] << ‘ ' 
<< (radToDeg * drPosition.sample[3]) << ‘' ' 
<< (radToDeg * drPosition.sample[4]) << ‘ ' 
<< (radToDeg * drPosition.sample[5]) << ‘ ' 
<< GdrPosition.sample[{6] << ‘' ' 
<< (radToDeg * drPosition.sample[7]) << ' ' 
<< drPosition.current[0] << ' ' 
<< GrPosition.current[1] <<endl; 


JL REEREEER LAER LE REAR REL ALES ERE REALL AES EELS ERERAE RELA EAE RAR ERASE RR AAS SS 


PROGRAM: writeGpsData 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Writes a raw GPS message to a binary output file for 
post processing. 

RETURNS : void 

CALLED BY: navPosit (nav.cpp) 

CALLS: None 


ee ee ee ee ee ee ee ee ee eee ee, 
ye 

void 

navigator: :writeGpsData(const GPSdata& satPosition) 


{ 
for( int j = 0; j < GPSBLOCKSIZE; j++) { 
putc(satPosition[j], rawData) ; 


} 
ay 


// end of file toefish.cpp 
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C. NAV.H 


#ifndef _NAVIGATOR_H 
#define _~NAVIGATOR_H 
#include <stdio.h> 
#include <fstream.h> 
#include <iostream.h> 
#include <math.h> 
#include <dos.h> 
#include "“toetypes.h" 
#include “globals.h" 
#include “gps.h" 
#include "ins.h" 


[RREEELAELE LEER ERLE RAS SE ELLE ERA ERER LES ELE LE LER EAR LAE AERA ELEN ESS 





CLASS: navigatorClass 
AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995, Modified January 1997 


FUNCTION: Combines GPS and INS information to return the current 
estimated position. 


Se Kee tee Fe RE te KK FI TK RK KK KR RK KK KKK KEKE KE KK KEK KKK KEE KKK KEKE EAR EEK KK / 
class navigatorClass { 
public: 
// Constructor, initializes object slots 
navigatorClass() : gpsSpeedSum(0.0), insSpeedSum(0.0) 
{ cerr << "\nconstructing navl" << endl; }; 


--navigatorClass() {} // Destructor 


// provides the navigator's best estimate of current position 
Boolean navPosit (stampedSample&) ; 


// Initialize the navigator 
Boolean initializeNavigator (stampedSample&) ; 


void userInitNav(stampedSample&) ; // Allows user to initialize nav 
private: 

double gpsSpeed, insSpeed, gpsSpeedSum, insSpeedSum; 

insClass insl; // ins object instance 

gpsClass gpsl; // gps object instance 


// Obtains system time to utilize for origin 
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double getSysteémTime() ; 
latLongMilSec origin; // lat-long of navigational origin 


// Returns the position in Miliseconds 
latLongMilSec getMilSec(const GPSdata&) ; 


// Returns the position in degrees, minutes, seconds and milisecs 
latLongMilSec latLongToMilSec(const latLongPosition&) ; 


// Convert position in milSec to degress, minutes, seconds and milsec 
latLongPosition milSecToLatLong(const latLongMilSec&) ; 


// Convert xy (grid) position to lat long 
latLongMilSec gridToMilSec(const grid&) ; 


// Converts lat/long to xy position 
grid milSecToGrid(const latLongMilSecs&) ; 


// Parses and returns the time of a GPS message. 
double getGpsTime(const GPSdata& rawMessage) ; 


// Parses and returns the velocity in fps of a GPS message. 
double getGpsVelocity(const GPSdata& rawMessage) ; 


a * 
Jy 


#endif 
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D. NAV.CPP 


#include <signal.h> 
#include <dos.h> 
#include <time.h> 
#include “nav.h" 





#define SIGFPE 8 // Floating point exception 


[PEELE ERE ERE REEERE ELE ARARES EES ERA EEA EEELERAERRELA ESSER AAS RE 


PROGRAM: navPosit 


| AUTHOR: Eric Bachmann, Dave Gay 

DATE: . 11 July 1995 

| FUNCTION: Provides the navigator's best estimate of current position. 
Attempts to obtain GPS and INS position fixes from the gps 
and ins objects and copies the most accurate fix available 
into the input argument ‘navPosition'. Sets a return 
flag to indicate whether a valid fix was obtained. 

RETURNS: TRUE, a valid position fix is in the variable ‘navPosition’. 
FALSE, otherwise. 

CALLED BY: towfish.cpp (main) 


CALLS: gpsPosition (gps-h) gridToMilSec (nav-h) 
correctPosition (ins.h) milSecToGrid (nav.h) 
insPosition (ins.h) milSecToLatLong (nav.h) 
getMilSec (nav.h) writeScriptPosit (nav.h) 


KKK KK KR KR KKK KKK KKK RKEKRKKEK KKK KEK KR KKK KR RRR RK KKK KK RK KKK KKK RK KKK KEKE ERE KEKE EK / 


void fpeNavPosit(int sig) 
{if (sig == SIGFPE) cerr << "floating point error in navPosit\n";} 


Boolean navigatorClass::navPosit (stampedSample& posit) 


{ 
Signal (SIGFPE, fpeNavPosit); 


latLongMilSec gpsMilSec; // the latest GPS position in milseconds 
latLongMilSec insMilSec; // the latest INS position in milseconds 
// Attempt to get the INS and GPS positions 

posit.insFlag = insl.insPosition (posit) ; 

posit.gpsFlag = gpsi.gpsPosition(posit.satPosition) ; 

// INS and GPS positions obtained? 
if (posit.insFlag && posit.gpsFlag) { 

// Parse position from GPS messsage 


gpsMilSec = getMilSec(posit.satPosition) ; 


posit.est = milSecToGrid(gpsMilSec) ; 
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// Pass GPS position to INS object for navigation corrections. 
_insl.correctPosition (posit, getGpsTime (posit.satPosition) ); 


// Convert position in milisec to latitude and longitude. 
posit.navLatLong = milSecToLatLong(gpsMilSec) ; 


return TRUE; 
} rs 


else { 
if (posit.insFlag) { // Only INS position obtained? 


insMilSec = gridToMilSec(posit.est) ; 
posit.navLatLong = milSecToLatLong(insMilSec) ; 
insSpeed = posit.sample[6]; 
return TRUE; 

} 


else { 
if (posit.gpsFlag) { // Only GPS position obtained? 


// Parse position from GPS messsage 
gpsMilSec = getMilSec(posit.satPosition) ; 
posit.est = milSecToGrid(gpsMilSec) ; 


// Pass GPS position to INS object for navigation corrections. 
insil.correctPosition(posit, getGpsTime(posit.satPosition) ); 


// Convert position in milisec to lat/long. 


posit.navLatLong = 
milSecToLatLong (getMilSec(posit.satPosition) ); 


return TRUE; 
} 


else { 
return FALSE; // No new position available 


LEE EELEA SLE AA SALES LALA AEA LAR CEA LAE EAE A RELL DN SLE RR ee ee 


PROGRAM: initializeNavigator 

AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 

DATE: 11 July 1995 

FUNCTION: Obtains an initial GPS fix for use as a navigational origin 


for grid positions used by the INS object. Saves the origin 
and passes it to the INS object in latLong form. 


RETURNS : TRUE 


CALLED BY: towfish (main) 

CALLS: gpsPosition (gps.cpp) writeGpsData (nav.cpp) 
correctPosition (ins.cpp) getMilSec (nav.cpp) 
writeInsData (nav.cpp) milSecToGrid (nav.cpp) 


He KR EO KR RK RK KR EKER KR RRR KR KEK KERR KKK KEKE KK KEK KR KKK KEK KK KEK RK KKK KEK KK EEK / 
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Boolean navigatorClass::initializeNavigator(stampedSample& posit) 


{ 
Boolean gpsFlag(FALSE) ; 


cerr << "Initializing Navigator." << endl; 
cerr << " Initializing GPS." << endl; 


// boop until an initial GPS fix is obtained. 
for (int i=1; ((i < 100) && (gpsFlag == FALSE)) ; i++) {¢ 
if (gpsl.gpsPosition(posit.satPosition)) { 
gpsFlag = TRUE; 


} 
else { 

delay (500); 
} 


} 
if (gpsFlag == FALSE) { 
cerr << "\nWARNING: UNABLE TO OBTAIN INITIAL GPS FIX!" << endl; 


userlInitNav (posit); 





} 

else { 
cerr << " GPS initialization complete." << endl; 
// Save navigational origin for later grid position conversions. 
origin = getMilSec(posit.satPosition) ; 
// Pass time of first GPS fix to INS object initialization routine. 
insl.insSetUp(getGpsTime (posit.satPosition), posit); 

} 


cerr << "Navigator initialization complete." << endl; 


return TRUE; 


[EEEEELELAELAES BELLE EAE EELERE RAE LR ERE SA TREE RAE LER ALA LS NESS ee ee 


PROGRAM: userInitNav 

AUTHOR: Rick Roberts 

DATE: 03 November 1996 

FUNCTION: Allows user to input current position and initialize 
nav if no gps fix is available. (ie for testing) 

RETURNS : void 

CALLED BY: initializeNavigator 

CALLS: getMilSec (nav.cpp), getSystemTime (nav.cpp) 


a nn er rn ee ee ee ee | 
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void navigatorClass: :userInitNav(stampedSample& posit) 


{ 


int choice; 


cerr << "\nEnter a 0 to enter posit and continue without GPS" 
<< "\nEnter a 1 to continue without GPS or initial posit, or" 
<< "\nEnter a 2 to exit: " << endl; 

cin >> choice; 


if (choice == 0) { 

cerr << "\nEnter current position in the following format: " << endl; 
cerr << "Latitude: (36, Enter, 35 Enter, 41.5 Enter)" << endl; 

cin >> posit.navLatLong.latitude.degrees; 

cin >> posit.navLatLong.latitude.minutes; 

cin >> posit.navLatLong.latitude.seconds; 

cerr << "Longitude: (-121, Enter, 52, Enter, 30.2, Enter)" << endl; 
Cin >> posit.navLatLong.longitude.degrees; 

Cin >> posit.navLatLong.longitude.minutes; 

cin >> posit.navLatLong.longitude.seconds; 


} 


else if (choice == 2) { exit(1); } 


// Save nav origin for later grid position conversions 
origin = latLongToMilSec (posit .navLatLong) ; 


// Pass system time of initialization to ins object 
insl.insSetUp(getSystemTime(), posit); 
} 


PELELELLEE EEE LALLA ERA LE LEAL SARE LEE EEE AAEA ASA EEA EEL AEE EE SRR SA 


PROGRAM: latLongToMilSec 

AUTHOR : Rick Roberts 

DATE : 22 January 1997 

FUNCTION: Converts a position expressed in latitude and longitude 
degrees, minutes and seconds to mili seconds & returns it. 

RETURNS : latLongMilSec 

CALLED BY: userInitNav 

CALLS: none 


KK KKK IK KK KR KK IK KKK KR KKK KKK KE KK KEK KKK KK KEE KKEKEKEREEKKKKKRKEKEKEKREKREKKERKEER / 


latLongMilSec navigatorClass::latLongToMilSec(const latLongPosition& 
latLong) 


{ 
latLongMilSec milSec; 
double degrees, minutes, seconds; 


milSec.latitude = (latLong.latitude.degrees * DEGREES_TO.MSECS) + 
(latLong.latitude.minutes * MINS_TO_MSECS) + 
(latLong.latitude.seconds * 1000.0); 


milSec.longitude = (latLong.longitude.degrees * DEGREES_TO_MSECS) + 


ae) 








(latLong.longitude.minutes * MINS_TO_MSECS) + 
(latLong.longitude.seconds * 1000.0); 
return milSec; 
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PROGRAM: getSystemTime 

AUTHOR: Rick Roberts 

DATE: 03 November 1996 

FUNCTION: Obtains system time to utilize for origin. 
RETURNS : double (origin time in seconds) 

CALLED BY: useriInitNav 

CALLS: dos time function 
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double navigatorClass: :getSystemTime () 


{ 


dostime_t* sysTime; // pointer to dos time structure 


_dos_gettime(sysTime) ; 


return double((sysTime->hour * 3600.0) + (sysTime->minute * 60.0) 
+ (sysTime->second) ); 
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PROGRAM: getMilSec 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Extracts a position in miliseconds from a Motorola (@@Ba) 
| position contained in the input argument ‘rawMessage’. 

RETURNS: The latitude and longitude in miliseconds. 


CALLED BY: navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 
CALLS: none. 
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latLongMilSec navigatorClass::getMilSec(const GPSdata& rawMessage) 
{ 

FOURBYTE temps4byte; 

latLongMilSec position; 


temps4byte = rawMessage[1i5]; 

temps4byte = (temps4byte<<8) + rawMessage[16]; 
temps4byte = (temps4byte<<8) + rawMessage[i17]; 
temps4byte = (temps4byte<<8) + rawMessage[i8]; 


position.latitude = temps4byte; 
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} 





temps 4byte = rawMessage[19]; 

temps4byte = (temps4byte<<8) + rawMessage[20]; 
temps4byte = (temps4byte<<8) + rawMessage[21]}; 
temps 4byte = (temps4byte<<8) + rawMessage[22]; 


position.longitude = temps4byte; 


return position; 
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PROGRAM: milSecToLatLong 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 . 

FUNCTION: Converts a position expressed totally in miliseconds to 
degrees, minutes, seconds and miliseconds. 

RETURNS: The position in degrees, minutes, seconds and miliseconds. 

CALLED BY: navPosit (nav.cpp) 

CALLS: none 
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latLongPosition navigatorClass::milSecToLatLong(const latLongMilSec& 
milSec) 


{ 


latLongPosition position; 
double degrees, minutes; 


degrees = (double)milSec.latitude * MSECS_TO_DEGREES ; 
position.latitude.degrees = (TWOBYTE) degrees; 


if(degrees < 0) { 

degrees = fabs(degrees); 
} 
minutes = (degrees - (TWOBYTE)degrees) * 60.0; 
position.latitude.minutes = (TWOBYTE) minutes; 
position.latitude.seconds = (minutes - (TWOBYTE)minutes) * 60.0; 


degrees = (double)milSec.longitude * MSECS_TO_DEGREES; 
position.longitude.degrees = (TWOBYTE) degrees; 


if(degrees < 0) { 

degrees = fabs(degrees) ; 
} 
minutes = (degrees - (TWOBYTE)degrees) * 60.0; 
position.longitude.minutes = (TWOBYTE)minutes; 
position.longitude.seconds = (minutes - (TWOBTE)minutes) * 60.0; 


return position; 
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PROGRAM: gridToMilSec 

AUTHOR: Eric Bachmann, Dave Gay 

DATE : 11 July 1995 

FUNCTION: Convert a grid position to a latitude and longitude in mili- 
“seconds and returns the result. 

RETURNS: The latitude and longitude in miliseconds. 

CALLED BY: navPosit (nav.cpp) 

CALLS: none 


KKK RK RR KR KKK KEKE KEE KKK KR KEKE KKK RE KE KEKE KK KEKE KERR EKER K KEE KEKE KKEEKKEEKER / 


void fpeGridToMilSec(int sig) 
{if (sig == SIGFPE) cerr << "floating point error in gridToMilSec\n"; } 


latLongMilSec navigatorClass::gridToMilSec(const grid& posit) 
{ 

signal(SIGFPE, fpeGridToMilSec) ; 

latLongMilSec latLong; 


// converts grid in ft to latitude 

latLong.latitude = origin.latitude + (posit.x / LatToFt); 

// converts grid in ft to longitude 

latLong.longitude = origin.longitude + 
HemisphereConversion * (posit.y / LongToFt) ; 

return latLong; 
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PROGRAM: milSecToGrid 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Convert a latitude and longitude expressed in milseconds to 
a grid position in xy coordinates in feet from the origin. 

RETURNS: The grid position 

CALLED BY: navPosit (nav.cpp), initializeNavigator (nav.cpp) 

CALLS: none 

COMMENTS: altitude is always assumed to be zero. 
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grid navigatorClass::milSecToGrid(const latLongMiiSec& posit) 


{ 
grid position; 
position.x = (posit.latitude - origin.latitude) * LatToFt; 
position.y = HemisphereConversion * 
(posit.longitude - origin.longitude) * LongToFt; 
position.z = 0; 
return position; 
} 
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PROGRAM: getGpsTime 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Parse the time of a gps message. 

RETURNS: The time of the gps message in seconds 

CALLED BY: navPosit (nav.cpp), initializeNavigator (nav.cpp) 
CALLS: none 
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double navigatorClass::getGpsTime(const GPSdata& rawMessage) 
{ 

UNS IGNED_ONEBYTE tempchar, hours, minutes; 

UNS IGNED_FOURBYTE tempudbyte; 

double seconds; 


hours = rawMessage[8]; 
minutes = rawMessage[9]; 


tempchar = rawMessage[10]; 
tempu4byte = rawMessage[11]; 
tempu4byte = (tempu4byte<<8) + rawMessage[12]; 
tempu4dbyte = (tempu4dbyte<<8) + rawMessage[13]; 


tempu4dbyte = (tempudbyte<<8) + rawMessage([14]; 
seconds = (double)tempchar + (( (double) tempu4byte) /1.0E+9); 


return hours * 3600.0 + minutes * 60.0 + seconds; 
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PROGRAM: getGpsVelocity 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Parse the velocity out of a gps message. 

RETURNS: The velocitiy of the gps message in feet per second 
CALLED BY: navPosit (nav.cpp), initializeNavigator (nav.cpp) 
CALLS: none 
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double navigatorClass::getGpsVelocity(const GPSdata& rawMessage) 


{ 
UNSIGNED_ONEBYTE tempchar=rawMessage [31]; 


return (double) (3.2804 * ((tempchar << 8) + rawMessage[32]) / 100.00); 


} 
// end of file nav.cpp 
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E. GPS.H 


#ifndef _GPS_H 
#define _GPS_H 


#include <iostream.h> 
#include <fstream.h> 
tinclude <conio.h> 


#include "“toetypes.h" 
#include "“globals.h" 
#include "gpsPort.h" 
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CLASS: gpsClass 
AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995, last modified January 1997 


FUNCTION: Reads GPS messages from the GPS buffer. Checks for valid 
checksum and minimun number of satellites in view. 


fe ek ee ee TR KK TRE KE IKK RIK KK IKK I KTR IK KK KKK KR RRR KEK KK RK KKK KERR / 
class gpsClass { 
public: 
// Class constructor and destructor 
gpsClass() { cerr << "\nconstructing gpsl" << endl; }; 


~gpsClass({) {} 


// returns the latest gps position and a flag 
Boolean gpsPosition(GPSdata&) ; 


private: 


// calculates the check sum of the message 
Boolean checkSumCheck(const GPSdata) ; 


3 


#endif 
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GPS.CPP 


#include <math.h> 
#include “gps.h" 


Ld 
ff 


instantiates serial port communications on comml, global to allow 
interrupt processing, cleanup to function properly 


gpsPortClass portl; 
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NAME : gpsPosition 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: . 11. July 1995 


FUNCTION: Determines if an updated gps position message is available 
and copies it into the input argument ‘rawMessage'. If the 
message has a valid checksum and was obtained with at least 

three satelites in view, a 'TRUE' is returned to the caller, 
indicating that the message is valid. 

RETURNS : TRUE, if a valid position message is contained in the 
input argument. 

CALLED BY: navPosit (navigator-h) 

CALLS: Get (buffer.h) 
checkSumCheck (gps.-h) 
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Boolean gpsClass::gpsPosition(GPSdata& rawMessage) 


{ 


unsigned long Mask(4); 
if (port1l.Get(rawMessage)) { 
// Check for a valid check sum and more the 3 satelites and DGPS 
“return Boolean((checkSumCheck(rawMessage)) && (rawMessage[39] > 3) 
&& ((rawMessage[GPSBLOCKSIZE - 4] & Mask) == Mask)); 
} 
else { 
return FALSE; // No updated position is available. 
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PROGRAM: checkSumCheck 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: id DULY: 1995 
FUNCTION: Takes an exclusive or of bytes 2 through 78 in a Motorola 
format (@@EA) position message and compares it to the 
: checksum of the message. 
RETURNS : TRUE, if the message contains a valid checksum 


CALLED BY: gpsPosition (gps) 
CALLS: none 
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Boolean gpsClass::checkSumCheck(const GPSdata newMessage) 


{ 
BYTE chkSum(0); 


for (int i = 2; i < GPSBLOCKSIZE - 3; i++) { 
chkSum *= newMessage[i]; | 


} 


return Boolean(chkSum == newMessage[GPSBLOCKSIZE - 3]); 


} 
// end of file gps.cpp 


G. INS.CFG 


//Konel 
//Kone2 
//Ktwo 
//Kthreel 
//Kthree2 
//Kfourl 
//Kftour2 
0 //tau 


eo Oo om cS 
COMM UWA NR Fe 


Qo + 
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H. INS.H 


#ifndef _INS_H 
#define _INS_H 


#include <time.h> 
#include <math.h> 
#include <dos.h> 
#include <stdio.h> 
#include <conio.h> 
#include <fstream.h> 
#include <iostream.h> 


#include "“toetypes.h" 
#include "globals.h" 
#include “sampler.h" 
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CLASS: insClass 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 


FUNCTION: Takes in linear accelerations, angular rates, 
heading information and uses Kalman filtering techniques to 


return a dead reconing position. 
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class insClass { 


public: 
_insClass(); // Constructor, initializes gains 
~insClass() {} // destructor 
Boolean insPosition(stampedSample&); // returns ins est. position 


// Updates the x, y and z of the vehicle posture 
void correctPosition(stampedSample&, double); 


// Sets posture to the origin and develops initial biases 


void insSetUp(double, stampedSample&) ; 


private: 
float posture[6]; // ins estimated posture (x y z phi theta psi) 
double velocities[6]; // ins estimated linear and angular velocities 
// x-dot y-dot z-dot phi-dot theta-dot psi-dot 
float current [3]; // ins estimated error current 


// (x-dot y-dot z-dot) 
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float lastGPStime; // time of last gps position f1x 


int tau; // filter time constant 
samplerClass saml; // sampler instance 


matrix rotationMatrix; // body to euler transformation matrix 


double biasCorrection[3]; // Software corrections, IMU rate sensors 


// Kalman filter gains. 
float Konel, Kone2, Ktwo, Kthreel, Kthree2, Kfourl, Kfour2; 


// Transforms body coords to earth coords, removes gravity component 
void transformAccels (double[]); 


// Transforms water speed reading to x and y components 
void transformWaterSpeed (double, double[]); 


// Tranforms body euler rates to earth euler rates. 
void transformBodyRates (double[]); 


// Euler integrates the accelerations and updates the velocities 
void updateVelocities (stampedSample&) ; 


// Euler integrates the velocities and updates the posture 
void updatePosture (stampedSample&) ; 


// Builds the body to euler rate matrix 
matrix buildBodyRateMatrix(); 


// Builds the body to earth rotation matrix 
void buildRotationMatrix(); 


// Calculates the imu bias correction during set up 
void calculateBiasCorrections (stampedSample&) ; 


// Applies bias corrections to a sample 
void applyBiasCorrections (stampedSample&) ; 


// Reads filter constants from ‘ins.cfg' 
void readInsConfigFile(); 


a 


// Post multiply a matrix times a vector and return result. 
vector operator* (matrix&, double[]); 


#endi£t 
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I. INS.CPP 


#include <iostream.h> 

#include <signal.h> 

#include “ins~.h" 

#define SIGFPE 8 // Floating point exception 
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PROGRAM : insClass (constructor) 
AUTHOR : Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995 


FUNCTION: Constructor initializes kalman filter gains and linear and 
angular velocities. 

RETURNS : nothing 

CALLED BY: navigator class 

CALLS: none 
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insClass::insClass() : Konel(0.5), Kone2(0.5), Ktwo(0.6), Kthreel(0.5), 
Kthree2(0.5), Kfourl(0.5), Kfour2(0.5), tau(1000) 


cerr << "\nconstructing insl" << endl; 


readInsConfigFile(); // Read the config file 
velocities[0] = 0.0; | ee ae (oe! 

velocities[i] = 0.0; // y aot 

velocities[2] = 0.0; // z aot 

velocities[3] = 0.0; // pri dot 

velocities[4] = 0.0; // theta dot 
velocities[5] = 0.0; // psi dot 

posture[0] = 0.0; // Set posture to straight and level at the origin. 
posture[1] = 0.0; 

posture (2]):= 0.0; 

posture[3] = 0.0; 

posture[4] = 0.0; 

posture[5] = 0.0; 

current[0} = 0.0; // Initialize error current to zero 
current[1] = 0.0; 

current[2] = 0.0; 


105 








[REREEAEREAE EEEEAERERER EERE EER AEERE REE ER REAR RAR ERER ERE E EEA ELSES ER EES 


PROGRAM: insPosit 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: Make dead reckoning position estimation using kalman 
filtering. Inputs are linear accelerations, angular rates, 
speed and heading. Primary input data is obtained from a 
sampler object via the getSample method. This data is stored 
in the sample filed of a stampedSample structure called 
newSample. The sample field is then used as a working 
variable as the linear accelerations and angular rates it 
contains are converted to earth coordinates and integrated 
to determine current velocities and posture. The data 1s 
complimentary filtered against itself, speed and magnetic 
heading. 
RETURNS : position in grid coordinates as estimated by the INS 
CALLED BY: navPosit (nav.cpp) 
CALLS: getSample (sampler.cpp) 
findDeltaT (ins.cpp) 
transformBodyRates (ins.cpp) 
buildRotationMatrix (ins.cpp) 
transformAccels (ins) 
transformWaterSpeed (ins) 
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void fpeInsPosit (int sig) 
{if (sig == SIGFPE) cerr << "floating point error in insPosit\n";} 


Boolean insClass::insPosition(stampedSample& newSample) 


{ | 
signal (SIGFPE, fpeInsPosit) ; 


double thetaA, phiA, xIncline, yiIncline; // Working variables 


double waterSpeedCorrection[3]j; // Filter correction for drift 
// and water speed 


if (saml.getSample(newSample)) { 


applyBiasCorrections (newSample) ; 


newSample.rawSample[0] = newSample.sample[0]; 
newSample.rawSample[1i] = newSample.sample[1]; 
newSample.rawSample[2] = newSample.sample[2]; 
newSample.rawSample[3] = newSample.sample[3]; 
newSample.rawSample[4] = newSample.sample[4]; 
newSample.rawSample[5] = newSample.sample[5]; 
newSample.rawSample[6] = newSample.sample[6]; 
newSample.rawSample[7] = newSample.sample[7]; 


xIncline = newSample.sample[0] / GRAVITY; 
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yIncline = (newSample.sample[1] - 
(newSample.sample[5] * newSample.sample[6]) ) 
/ (GRAVITY * cos(posture[4])); 


if (fabs(yIncline) > 1.0) { 
static int inclineCount (0); 


gotoxy (1,24) ; 
cerr << "Inclination errors: " << ++inclineCount << endl; 
. return FALSE; 
} 
thetaA = asin(xIncline) ; // Calculate low freg pitch and roll 
phiA = -asin(yIncline) ; 


// Transform body rates to euler rates. 
trans formBodyRates (newSample.sample) ; 


// Calculate estimated roll rate (phi-dot). 
velocities[3] = newSample.sample[3] + Konel * (phiA - posture[3}); 
// Calculate estimated pitch rate (theta-dot). 
velocities[4] = newSample.sample[4] + Kone2 * (thetaA - posture[4]); 
// Calculate estimated heading rate (psi-dot). 
velocities[{5] = 
newSample.sample[5] + Ktwo * (newSample.sample[7] - posture[5]); 


// integrate estimated angular rates to obtain angles 
posture[3] += newSample.deltaT * velocities[3];// pitch rate to angle 
posture[4] += newSample.deltaT * velocities[4]; // roll rate to angle 
posture[5] += newSample.deltaT * velocities[5]; // yaw rate to angle 


buildRotationMatrix(); 


// Transform accels to earth coordinates 
transformAccels (newSample.sample) ; 


// Transform water speed to earth coordinates 
transformWaterSpeed (newSample.sample[6], waterSpeedCorrection) ; 


// Subtract out previous velocity and apply statistical gain 
waterSpeedCorrection[0] = 


Kthreel * (waterSpeedCorrection[0] - velocities[0]); 
waterSpeedCorrection[1i] = 
Kthree2 * (waterSpeedCorrection[1] - velocities[1]); 


// Determine filtered accelerations 
newSample.sample[0] += waterSpeedCorrection[0]; 
newSample.sample[1] += waterSpeedCorrection[1]; 


// Integrate accelerations to obtain velocities 


velocities{0] += newSample.sample[{0] * newSample.deltaT; 
velocities[1] += newSample.sample[1] * newSample.deltaT; 
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velocities[2] += newSample.sample[2] * newSample.deltaT; 


J/ Integrate velocities to obtain posture 

posture[0] += (velocities{0] + current[0]) * newSample.deltaT; 
posture[1] += (velocities[1] + current[1]) * newSample.deltaT; 
posture[2] += velocities[2] * newSample.deltaT; 


newSample.sample[0] = posture[0]; 
newSample.sample[{1] = posture[i]; 
newSample.sample[2] = posture[2]; 
newSample.sample[3] = posture[3]; 
newSample.sample[4] = posture[4]; 


newSample.sample[5] = posture[5]; 


newSample.est.x = posture([0]; 
newSample.est.y posture[1]; 
newSample.est.z = posture[2]; 


It 


return TRUE; 
} 


else { 
return FALSE; // New IMU information was unavailable. 
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PROGRAM: correctPosition 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: Reinitializes the INS based on a known position and computes 
apparent current based on past accumulated errors of the INS. 
It is called by the navigator each time a new GPS (true) fix 
is obtained. 





RETURNS: void 
CALLED BY: navPosit (nav) 
CALLS: none 
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void 
insClass: :correctPosition(stampedSample& posit, double positTime) 


{ 
double deltaT; 


if (positTime < lastGPStime) { // Correct for new day if necessary 
positTime += 86400; 

} 

deltaT = positTime - lastGPStime; // Find time since last gps fix. 
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// Determine INS error since last gps fix 
double deltaX = posit.est.x - posture[0]; 
double deltaY = posit.est.y - posture[1]; 


// Reinitialize posture to known position (gps fix) 


posture[0] = posit.est.x; 
posture[1] = posit.est.y; 
posture[2] = 0.0; // Unit is assumed to be on the surface 


// Add gain filtered error to previous errors 
posit.current[0] = current[0] += Kfourl * (deltaX / deltaT); 
posit.current[1] = current[1] += Kfour2 * (deltaY / deltaT); 


// Save the time of the gps fix for next calculation 
lastGPStime = positTime; 
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PROGRAM: insSetUp 
AUTHOR: Eric Bachmann, Dave Gay 
DATE: 1. July L995 
FUNCTION: Initializes the INS system. Sets the posture to the origin. 
Initializes the heading using magnetic compass information. 
Initializes the last GPS fix and last IMU information times. 
RETURNS: void 
CALLED BY: initializeNavigator (nav) 
CALLS : calculateBiasCorrections (ins) 
getSample (sampler) 
buildRotationMatrix (ins) 
transformWaterSpeed (ins) 
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void fpeInsSetUp(int sig) 
{if (sig == SIGFPE) cerr << "floating point error in inSetUp\n"; } 


void insClass::insSetUp(double originTime, stampedSample& posit) 
{ 

cerr << " Initializing INS." << endl; 

signal (SIGFPE, fpeInsSetUp) ; 


saml.initSampler(); // Initialize the sampler 
calculateBiasCorrections (posit) ; // set imu biases 
posture[5] = posit.sample[7]; //set initial true heading 
buildRotationMatrix(); //set initial speed 


transformWaterSpeed(posit.sample[6], velocities) ; 
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posit.current[0] = 0.0; 
posit.current[1] = 0.0; 
posit.current[2] = 0.0; 
lastGPStime = originTime; // initialize times 


cerr << " INS initialization complete." << endl; 
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PROGRAM: transformAccels 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Transforms linear accelerations from body coordinates to 
earth coordinates and removes the gravity component in the 
z direction. 

RETURNS: void 

CALLED BY: navPosit 

CALLS: none 
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void insClass::transformAccels (double newSample[]) 


{ 


vector earthAccels; 

newSample[0] -= GRAVITY * sin(posture[4]); 

newSample[1] += GRAVITY * Sin(posture[3]) * cos (posture[4]); 
newSample[2] += GRAVITY * cos(posture[3]) * cos (posture[4]); 


earthAccels = rotationMatrix * newSample; 


newSample[0] = earthAccels.element [0]; 
newSample[1] = earthAccels.element{1]; 
newSample[2] = earthAccels.element [2]; 


[LEER EAE SAAR TALES RARER EL ETS A AES RES ENE a eRe Se ene 


PROGRAM: transformWaterSpeed 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11. July 2995 

FUNCTION: Transforms water speed into a vector in earth coordinates and 
returns them in the speedCorrection variable. 

RETURNS : void 

CALLED BY: navPosit 

CALLS: none 
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void insClass::transformWaterSpeed (double waterSpeed, double 
speedCorrection[] ) 


{ 
double water[3] = {waterSpeed, 0.0, 0.0}; 
vector waterVelocities = rotationMatrix * water; 
speedCorrection [0] = waterVelocities.element [0]; 
speedCorrection [1] = waterVelocities.element [1]; 
speedCorrection [2] = waterVelocities.element [2]; 
} 


LRLEALELEEREAAECS REL ALLE SS EASA AA EELS ON ARSE A TRAE SERA SESE Per eee 


PROGRAM: transformBodyRates 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Tranforms body euler rates to earth euler rates 
RETURNS : none 

CALLED BY: insPosit 

CALLS: buildBodyRateMatrix 


whe ee Ke SR IE KTR ICTR KE TOK TECK TBR KK KK KK I KK TK A KK IK KKK KKK KEK KKH KEE KKK KKK KKK EKER / 


void insClass::transformBodyRates (double newSample[]) 


{ 
vector earthRates = buildBodyRateMatrix() * &(newSample[3]); 
newSample[3] = earthRates.element[0]; 
newSample[{4] = earthRates.element [1]; 
newSample[5] = earthRates.element [2]; 
} 


LEER LELELAEL LALA LEAL ALES SEES PARAS EA ELL EL EELS En RO ee eR Ree 


PROGRAM: buildBodyRateMatrix 


AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Builds body to Euler rate translation matrix. 
RETURNS : rate translation matrix 

CALLED BY: insPosit 

CALLS: none 


eK TK IK KK KK KR KK KK RK KK KKK KKK KK KEKE KKK KR EK RRR EKER EEK KEKE KEKE KEKEEKE / 


matrix insClass: :buildBodyRateMatrix() 
{ 


matrix rateTrans; 


float tth = tan(posture[4]), 
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Ke KKK KKK KK KKK KK KK KK KK RK KR KEKE KEKE KEE KK EERE KEE KEK KEE KEK KR RR KE KEK RER / 





sphi = sin(posture[3]), 

cphi = cos(posture[3]), 

cth = cos(posture[4’]); 
rateTrans.element[{0][0] = 1.0; 





rateTrans.element[0}[1] = tth * sphi; 
rateTrans.element[0][2] = tth * cphi; 
rateTrans.element[{[1][0] = 0.0; 


rateTrans.element[1][1] = cphi; 
rateTrans.element[1][2] = -sphi; 
rateTrans.element[2][0] = 0.0; 
rateTrans.element[2]{1] = sphi / cth; 
rateTrans.element[2][{2] = cphi / cth; 


return rateTrans; 


PROGRAM: buildRotationMatrix 


AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 . 

FUNCTION: Sets the body to earth coordinate rotation matrix. 
RETURNS : void 

CALLED BY: insPosit, insSetUp 

CALLS : none 


void insClass: :buildRotationMatrix() 


{ 


float spsi = sin(posture[5]), 
cpsi = cos(posture[5]), 

sth = sin(posture[4]), 

sphi = sin(posture[3}), 

cphi = cos(posture[3]), 

Cth: = -cos (pesture [4 ).)+> 


rotationMatrix. 
rotationMatrix. 
rotationMatrix. 
rotationMatrix. 
rotationMatrix. 
rotationMatrix. 
rotationMatrix. 
rotationMatrix. 
rotationMatrix. 


element {0} [0] 
element [0] [1] 
element [0] [2] 
element [i] [0] 
element [1] [1] 
element [1] [2] 
element [2] [0] 
element [2] [1] 
element [2] [2] 


epsi. * ei: 

{cepsi * sth * sphi) - (spsi * cphi); 
(cpsi * sth * cphi) + (spsi * sphi); 
spsi * cth; 

(cosi. * cphi) + (spsi. *-sth: *. spha); 
(spsi * sth * cphi) - (cpsi * sphi); 
-sth; 

cth* sphi-s 

ech. *-epnis 
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PROGRAM: postmultiplication operator * 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Post multiply a 3 X 3 matrix times a 3 X 1 vector and 
return the result. 

RETURNS : 3 X 1 vector 

CALLED BY: 

CALLS: None 


Be Fe KF KK KKK IT K KKK IKK KKK KK KK KK KKK EKER KEE KKK KEK KR KEKE EKK KR KEEKEKREEKRERERKEKKER / 


vector operator* (matrix& transform, double state[]) 


{ 


vector result; 

for (int i = 0; i < 3; i++) { 
result.element{i] = 0.0; 
for (int j = 0; j < 3; j++) { 


result.element[i] += transform.element[iJ[{j] * state[j]; 
} 
} 


return result; 


{EELLEEL ELAR EE ERE ALLEL LS SRE ERE EEL AA ELER ERS LAER SSE A EEA 


PROGRAM : calculateBiasCorrections 

AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 

DATE: 11 July 1995 

FUNCTION: Calculates the initial imu bias by averaging a number of 
imu readings. 

RETURNS : none 

CALLED BY: insSetup 

CALLS: none 


Be KEI KK KTR KI I RR I KK I RR KK KKK KERR KKK KKK KEKE KKK KKK KEKE KK KKK KKREK ERK / 


void fpeCalculateBiasCorrections(int sig) 
{if (sig == SIGFPE) cerr << “floating point error in 
CalculateBiasCorrections\n";} 


void insClass::calculateBiasCorrections (stampedSample& biasSample) 
{ 
signal (SIGFPE, fpeCalculateBiasCorrections) ; 
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int biasNumber(tau/10); 


// p roll rate 
// q pitch rate 
// xv yaw rate 


i 
Oo Oo © 


biasCorrection[0] 
biasCorrection[1] 
biasCorrection[2] = 


Oo Oo Oo 
=. ™*¢ =a 


for (int i = 0; i < biasNumber; i++) { 


// Find the average of the biasNumber packets 
while(!saml.getSample(biasSample)) {/* */}; 
biasCorrection[0] += biasSample.sample[3]/biasNumber; // roll-rate/ 


b# 
biasCorrection[1] += biasSample.sample[4]/biasNumber; // pitch-rate/ 


b# 
biasCorrection[2] += biasSample.sample[5]/biasNumber; // yaw-rate/b# 


} 


// set biasSample correction fields to new bias correction values 
// negative biasCorrection value is taken so biases are added to sensor 


values 
biasSample.bias[3] = biasCorrection[0] = -(biasCorrection{[{0]); 
biasSample.bias[4] = biasCorrection[1] = ~(biasCorrection[1]}); 
biasSample.bias [5] = biasCorrection[2] = -(biasCorrection[2]}); 


[RESELLER LEAEREEAEAER ELE ALRE ELA EL ERE LER ERE AS EREAALL ELS LA ESSE SL RE 


PROGRAM: applyBiasCorrections 

AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 

DATE: a. a ey > 

FUNCTION: Applies updated bias corrections to a sample. 
RETURNS: void 

CALLED BY: insPosit 

CALLS: none 


Fe TE TE DEI TOF KK FE KI KKK KKK KK KR KR KKK KK KEK KK KKK KKEKEKKEKREKEKEKEKKRKEEKKEEKEKERE / 


void insClass::applyBiasCorrections (stampedSample& posit) 


{ 
const float sampleWght (posit.deltaT/tau) ; 


const float biasWght(1 - sampleWght) ; 


//Calculate updated bias values 


biasCorrection[0] = (biasWght * biasCorrection[0]) 

- (sampleWght * posit-sample[3]); 
biasCorrection[1] = (biasWght * biasCorrection[1]) 

~ (sampleWght * posit.sample[4]); 
biasCorrection[2] = (biasWght * biasCorrection[2]) 


- (sampleWght * posit.sample[5]); 
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posit.sample[3] += biasCorrection[0]; //Apply the bias to the sample 
posit.sample[4] += biasCorrection[1]; 
posit.sample[5] += biasCorrection[2]; 


posit.bias[3] = biasCorrection[0]; //Save the bias to the sample 
posit.bias[{4] = biasCorrection[1]; 


posit.bias[5] = biasCorrection[2]; 


LREREREAELERELELELS PERERA EEARAREEE RAY AERA EAL EAS LARSEN RS RS Se 


PROGRAM: readInsConfigFile 

AUTHOR: Rick Roberts, Eric Bachmann 

DATE: 02 Nov 96 

FUNCTION: Reads filter constants from ‘ins.cfg' 
RETURNS : void 

CALLED BY: ins class constructor 

CALLS: none 


RK RR RR RK RRR KR RK KR RAK RRR RAK AK KEKE KEK KKK KKK KEK KKK REE EEK EK KEE KER KK / 


void insClass: :readInsConfigFile() 


{ 


cerr << "Reading ins configuration file." << endl; 
ifstream insCfgFile("“ins.cfg", los::1in); 


if(!insCfgFile) { 
cerr << "could not open ins configuration file!" << endl; 


else { 
char comment{[{128]; 


insCfgFile 
>> Konel >> comment 
>> Kone2 >> comment 
>> Ktwo >> comment 
>> Kthreel >> comment 
>> Kthree2 >> comment 
>> Kfourl >> comment 
>> Kfour2 >> comment 
>> ftau >> comment; 
} 
insCfgFile.close(); 
ae 
// end of file ins.cpp 
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J. SAM.CFG 


;pScale(roll) 
;qScale (pitch) 
;rScale (yaw) 
4 ;xAccelScale (pitch) 
4. ;yAccelScale(roll) 
4 - ;zAccelScale (yaw) 
27 | ;waterSpeedScale 


K. SAMPLER.H 


#ifndef _SAMPLER_H 
#define _SAMPLER_H 


#include <time.h> 
#include <math.h> 
#include <dos.h> 
#include <conio.h> 
#include <stdio.h> 
#include <fstream.h> 
#include <iostream.h> 


#include "toetypes.h" 
#include "“globals.h" 
#include "“a2d.h" 
#include "compass.h" 


#define MAX_SAMPLE_NUM 1000 


#define xyAccelLimit ONE_G // Max accell in x and y direction 
#define zAccelLimit 2 * ONE_G // Max accel in z direction 
#define rateLimit 0.872665 // Max rotational rate in radians 
#define speedLimit 25.3 // Max water speed 


#define headingLimit 2 * M_PI 


const int INBUFFSIZE = 512; 


J[RELAESEEA A ERELEERE ERE RELL RE EER AE EE LERLE ES ERELELE ER LLE REESE LES AREY SENOS 


CLASS: samplerClass 
AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995, last modified January 1997 


FUNCTION: Formats, timestamps, low pass filters and limit checks IMU, 
water-speed and heading information. 

COMMENTS: This class is extremely dependent upon the specific 
hardware configuration. It is designed to isolate the 
INS from these particulars. 


Bee eH RIK IK IK KKK IK KK LOK KR RK KR KK KKK KKK KKK KKK KK KE KKK KEK KEKE KEK KHER EK KEEEE RE / 
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class samplerClass -{ 


public: 
samplerClass(); // 
~samplerClass({) {} 
-Boolean initSampler() ; a 


fi checks for the arrival of a new 


Boolean getSample(stampedSample&) ; 


private: 


fioat 
float 
float 


float 
float 
float 


float 


compassClass compl; 
a2dClass a2dl; 


// stores incoming FIFO samples by 


pScale; es 
qScale; es 
rScale; ce 
xAccelScale; // 
yAccelScale; ie 
zAccelScale; ad 
waterSpeedScale; 


// 


float sample [MAX SAMPLE_NUM] [8]; 


int subSampleiIndex; 
int sampleIndex; 


int sampleCount; 


float samplePeriod; 


// 


ee 


// 





Class constructor, destructor 


Initializes Sampler 


sample and formats it 


roll 
pitch 
yaw 


pitch 
roll 
yaw 


// instantiate member compass object 


instantiate member a2d object 


channel 


counts channels 
indexes samples' array 


counts samples 


Boolean readSamples(stampedSample& newSample) ; 


void filterSample(stampedSample& newSample) ; 


void 


void 


void 


formatSample(stampedSample& newSample) ; 


increment (int& index) 


if (++index == 


decrement (int& index) 
if (--index < 0) index 
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MAX SAMPLE NUM) index 


0;} 


MAX _SAMPLE_NUM - 1;} 








// Reads filter constants from 'sam.cfg' 
void readSamplerConfigFile(); 


double pUnits(double angular) 


{ return 
(pScale * (((angular-2047.0) / 2047.0 ) * 50.0) * (M_PI/180.0));} 


double qUnits(double angular) 


{ return 
(qScale * (((angular-2047.0) / 2047.0 ) * 50.0) * (M_PI/180.0));} 


double rUnits(double angular) 


{ return 
(rScale * (((angular-2047.0) / 2047.0 ) * 50.0) * (M_PI/180.0));} 


double xAccelUnits(double linear) 
{ return (xAccelScale * ((linear-2047.0) / 2047.0 ) * GRAVITY) ;} 


double yAccelUnits(double linear) 
{ return (yAccelScale * ((linear-2047.0) / 2047.0 ) * GRAVITY) ;} 


double zAccelUnits (double linear) 


{ return 
(zAccelScale * ((linear-2047.0) / 2047.0) * (2.0 * GRAVITY) );} 


double depthUnits(double depth) 
{ return (((depth - 819.0) / (4095.0-819.0)) * 180.0);} 


double waterSpeedUnits (double speed) //feet per second 

{ return (waterSpeedScale * ((speed - 2047.0) / 2048.0) * 25.3);} 
ie 
#endif 
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L. SAMPLER.CPP 


#include “sampler.h" 


JLEREEEEREL AE EREREEEEERLERLRER ES EERREREE EEE ARERR EERES EAE EEL ESE EES 


PROGRAM: samplerClass Constructor 

AUTHOR : Eric Bachmann, Randy Walker, Rick Roberts 

DATE: 12 May 1995, last modified December 1996 

FUNCTION: Constructs saml, initializes default config values, calls 
readSamplerConfigFile to read any updated values. 

RETURNS: saml 

CALLED BY: insSetUp (ins.cpp) 

CALLS: readSamplerConfigFile 


KCK KKK IK KK TKR SK KK KK KKK KK KKK IT KT KK KEK KK IKKE KK KKK ERK KKK KEKE REE KKK EKER EEE / 


samplerClass::samplerClass () 
>: sampleIndex(0), subSampleIndex(0), 
samplePeriod(a2d1l.chent * a2dl.delta_t * 0.000001), 
pScale(0.0), qScale(0.0), rScale(0.0), 
xAccelScale(0.0), yAccelScale(0.0), zAccelScale(0-.0), 
waterSpeedScale (0.0) 


cerr << "\nconstructing sampler w/ a2d1, compl" << endl; 
readSamplerConfigFile(); 


LELFERELLESELLEEA EEAALLLELAELLERE AAS REEL EA AEA EE RELL ALA LAL ERA REN ERAT 


PROGRAM: initSampler 

AUTHOR: Eric Bachmann, Randy Walker, Rick Roberts 
DATE: 12 May 1995 

FUNCTION: Instantiates the compass A2D objects. 
RETURNS: TRUE 

CALLED BY: insSetUp (ins.cpp) 

CALLS: initCompass(), A2D member functions 


ee TKK KK KKK KEK KEK KEK KKK EK KK KEKE KE EKER KEK KEE KK REE ERK KEEKEKEKEEE EKER KEKE / 


Boolean samplerClass::initSampler () 

{ 
cerr << " Initializing Sampler" << endl; 
compi.initCompass (); 


cerr << " Initializing A2D." << endl; 


a2d1.initA2d(); 
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cerr << " “A2D initialization complete." << endl; 
cerr << " Sampler initialization complete." << endl; 


return TRUE; 


[BR RRER RHR ERHEER EERE KERR EE REREREREEER EEE KEE EERE LEBER EEE ERE ERE EEE LES 


PROGRAM: getSample 
AUTHOR: Fric Bachmann, Dave Gay 
DATE: 11 July 1995 
FUNCTION: Prepares raw sample data for use by the INS object 
RETURNS: TRUE, if a valid sample was obtained 
CALLED BY: insPosit (ins) 
insSetup (ins) 
CALLS : readSamples (sampler) 
filterSample (sampler) 
formatSample (sampler) 


nnn nnn Serr Se ee ee ee ee eee ee 


Boolean samplerClass: :getSample(stampedSample& newSample) 


{ 


if (readSamples(newSample)) { // checks for the arrival of a new sample 
filterSample(newSample) ; 
formatSample (newSample) ; 


return TRUE; 


return FALSE; // Sample packet not available 
} 


[RELEASE LEE LEE LARELERELE ESLER EEALEAS ERAS ORLA AR EEL ERES EAR LSE SEE eS 


PROGRAM: readSamples 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 12 May 1996 

FUNCTION: Retrieves all samples of the IMU, water speed, and depth 
that are present in the A2D FIFO until the FIFO is EMPTY. 
Calculates delta_t. 

RETURNS: TRUE - There were new samples pulled from the FIFO 
FALSE - There were no new samples 

CALLED BY: getSample 

CALLS: getFifoStatus(), getFifoData() 


Be KE KK TK IK KR TK RK RK KK KKK RK KK RE KERR EEK ERE KKK RK KERR KEKEKEKEEE ERE / 
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Boolean samplerClass: :readSamples(stampedSample& newSample) 


{ 


static int overflowCount (0); 


if (a2dl.getFifoStatus() == FULL) { // Did the FIFO overflow? 
gotoxy (1,19); 
cout << "FIFO Overflowed, #: " << ++overflowCount 


<< " reiniting a2d" << endl; 
a2dl.reinitA2d(); 
return FALSE; 


} 


if (a2dl.getFifoStatus() != EMPTY) { // Does the FIFO have new samples? 


sampleCount = 0; // Counts the number of samples taken 


while (a2dl.getFifoStatus() != EMPTY) { // Empty the FIFO 
sample [sampleIndex] [subSampleIndex++] = a2d1.getFifoData(); 


// Has it pulled one sample of each channel from the FIFO? 


if (subSampleIndex == 8) { 
subSampleIndex= 0; 
increment (sampleIndex) ; // set to record next sample 
++sampleCount ; 


if (sampleCount > 0) { 
// calculate time delta 
newSample.deltaT = sampleCount * samplePeriod; 
return TRUE; 
3 
else { 
return FALSE; 


// No full samples 


} 


else { // No new samples 


return FALSE; 
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PROGRAM: filterSample 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July..1995 

FUNCTION: Low pass filters eight closely spaced sets of sensor 

readings by summing the readings of each sensor and computing 

the average. 

RETURNS : void 

CALLED BY: getSample 

CALLS: none 


KKK KK KKK KKK KKK KKK KEKE KKK KKK EKER KK EK KKK EERE KKRERERERKEKKEKEEREKEKKEERK KEE / 


void samplerClass::filterSample(stampedSample& newSample) 


{ 
for (int i = 0; 1 < 8; i++) { 
newSample.samplefi] = 0; 
int j(sampleIndex) ; 


for (i = 0; i < sampleCount; 14+) { 


decrement (j); 


newSample.sample[0] += sample[j] [0] sampleCount ; 
newSample.sample[1] += sample{j3] [1] sampleCount; 
newSample.sample[2] += sample[j] [2] sampleCount ; 


sampleCount ; 
sampleCount ; 
sampleCount; 
sampleCount ; 
sampleCount; 


newSample.sample[3] += sample[j] [3] 
newSample.sample[4] += sample[j] [4] 
newSample.sample[5] += sample[j] [5] 
-newSample.sample[{6] += sample[j]{6] 
newSample.sample[7] += sample[ 3] [7] 


MS TA TR TSO SOONG UO 


[REELS ERREAELAR ERE ARERR eR RS Cee ee ee ee Re eR Pe ee Re ee On ee 


PROGRAM: formatSample 

AUTHOR: Eric Bachmann, Dave Gay 

DATE : 11 July 1995 

FUNCTION: Converts integers representing voltage readings into 
real world units which are useable by the INS. 

RETURNS: void 

CALLED BY:getSample 

CALLS: none 


KK KK KKK KKK KKK RR KEK KEKE KEK KE KEE KR KER ER KERR K KK EERE EKER KER KEERK KEKE E / 
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void samplerClass::formatSample (stampedSample& newSample) 


{ 


newSample.sample[0] xAccelUnits (newSample.sample[0]); 


H 


newSample.sample[1] = yAccelUnits(newSample.sample[1]); 
newSample.sample[2] = zAccelUnits(newSample.sample[2]); 
newSample.sample[3] = pUnits(newSample.sample[3]); 
newSample.sample[4] = qUnits(newSample.sample[4]); 
newSample.sample[5] = rUnits(newSample.sample[5]) ; 
newSample.sample[6] = waterSpeedUnits (newSample.sample[6]); 


newSample.sample[7] = compl.getHeading(); 


[RERERESEEEEERAELE ELSES ELLER AAEAELAAE RRA LALAS ERR EREAAEA SL EEAL EAE SE EE 


PROGRAM: readSamplerConfigFile 

AUTHOR: Rick Roberts, Eric Bachmann 

DATE: 02 Nov 96 

FUNCTION: Reads filter constants from ‘ins.cfg' 

RETURNS : void 

CALLED BY: ins class constructor 

CALLS: none 

COMMENTS : * Do not allow blanks in 'comment' section of sam.cfg * 


et RK KR KR KK IK IK KR KK KR KKK KERR ERK IK KK KK KKK KK EEE KKK KEK KEE KKK KEKE KEK EERE / 


void samplerClass: :readSamplerConfigFile() 


{ 
FILE *samCfgFile; 
if ((samCfgFile = fopen("sam.cfg", “r")) == NULL) { 
cerr << “could not open sampler configuration file!" << endl; 
} 
else { 
cerr << "\nReading Sampler configuration file." << endl; 


char line[128]; 


fscanf (samCfoFile, "%f%s",&pScale, line) ; 
cerr << "pScale: " << pScale << endl; 


fscanf(samCfgFile, "%f%s",&qScale, line) ; 
cerr << "qScale: " << qScale << endl; 


fscanf (samCfgFile, "%f%s",&rScale, line) ; 
cerr << "rScale: " << rScale << endl; 


fscanf (samCfgFile, "%f%s",&xAccelScale, line); 
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cerr << "xAccelScale: " << xAccelScale << endl; 


‘fscanf (samCfgFile, "$f%s",&yAccelScale, line) ; 
cerr << "yAccelScale: " << yAccelScale << endl; 


fscanf (samCfgFile, "%f%s",&zAccelScale, line) ; 
cerr << "zgAccelScale: " << zAccelScale << endl; 


fscanf (samCfgFile, "*f%s", &waterSpeedScale, line); 
cerr << "waterSpeedScale: " << waterSpeedScale << endl; 


} 
fclose(samCfgFile) ; 


} 
// end of file sampler.cpp 


M. COMPASS.H 


#ifndef _COMPASS_H 
#define _COMPASS_H 


#include <iostream.h> 
#include <fstream.h> 
#include <conio.h> 





#include “toetypes.h" 
#include “globalis.h" 
#include “compport.h" 


BYTE asciiToHex (BYTE) ; // conversion function prototype 


fEEELESEEEEEEERREAAE ELL EE REEL ELLE ALES AERELLELAL EARLE LENE AREAS SS 


CLASS: compassClass 
AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 
DATE : 11 July 1995, last modified January 1997 


FUNCTION: Reads compass messages from the compass buffer. Checks for 
valid checksum. Corrects heading for magnetic variation. 
Heading is continuous. There is no branch cut at 360 degrees. 


te ke Ke eK KK KK KKK KKK IKKE EK KK KR KK IK KR RK KKK KKK KEK KEK KEK KK KEKE KEE KEKE KER EK / 
class compassClass { 
public: 
// class constructor and destructor 


compassClass() : currentHeading (0.0) 
{ cerr << "Compass constructed." << endl; } 
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~compassClass() {(} 


float initCompass() ; // initialize currentHeading 
float getHeading(); // returns the latest heading 
private: 


// Maintains the most recently obtained heading. 
float currentHeading; 


// calculates the check sum of the message 
Boolean checkSumCheck(const compData) ; 


// Parses a selected field out of a compass message. 
float parseCompData(const compData, const BYTE); 


// Converts magnetic direction based on magnetic variation. 
float trueHeading(const float); 


// Returns the heading without branch cuts 
float continousHeading(const float); 

}; 

#endif 


N. COMPASS.CPP 


#include <math.h> 
#include <stdlib.h> 
#include “compass.h" 


// instantiates serial port communications on comm2, global to allow 


// interrupt processing, cléanup to function correctly 
compassPortClass port2; 


LLLEERELLALEEEAALA ELE EEA LALLA LAE AAA LELERELAL ERE LE SL AALS A OER ER oN 


NAME : initCompass 


AUTHOR : Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995 


FUNCTION: Determines if a valid compass message is held in the 
compass buffer and initializes currentHeading to that value. 
Will attempt 10 times with a built in delay and then exit 
with a warning if a valid heading is not obtained. 


RETURNS : currentHeading 

CALLED BY: INSsetUp (ins.cpp) 

CALLS : Get (buffer.h) parseCompData (compass.cpp) 
checkSumCheck (gps.-h) continuousHeading (compass.cpp) 


trueHeading (compass.cpp) 
Be KK KK KK KK RR KK KKK KKK KE KK KEK KEKE KERR ERR KKK REE KR EEK KKE EKER KEEKREKE EER / 
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float compassClass::initCompass() 





Serr a<-" Initializing Compass" << endl; 


Boolean compFlag (FALSE) ; 
fioat tempHeading; 
compData rawMessage; 


// try 10 times to get a valid message 
for (int i=1; ((i < 10) && (compFlag == FALSE)); i++ ) { 


if ((port2.headings.Get (rawMessage)) && (checkSumCheck (rawMessage) ) ) { 
tempHeading = parseCompData(rawMessage, ‘C') * degToRad; 
currentHeading = continousHeading (trueHeading (tempHeading) ); 
compFlag = TRUE; 
} 
else { // invalid message - delay 
delay (1000); 


} 


if (compFlag == FALSE) { 
cerr << "\nWARNING: UNABLE TO OBTAIN INITIAL COMPASS HEADING! " 
<< endl; 
delay (2000); 
} 
else { 
cerr << " Compass initialization complete." «<< endl; 
} 


4 


return currentHeading; 


FLEES AERA AEA A ELSA LES LEE ES LAA SEALE EE SA SE AS ELS eRe ee 


NAME : getHeading 

AUTHOR: Eric Bachmann, Dave Gay, Rick Roberts 

DATE: 11 July 1995 

FUNCTION: Determines if an updated compass message is available and 
copies it into the input argument ‘rawMessage'. If the 


message has a valid checksum, currentHeading is returned 
to the caller, currentHeading is also the default return. 


RETURNS : currentHeading 
CALLED BY: navPosit (navigator.h) 
CALLS: Get (buffer.h) 


checkSumCheck (compass.cpp) 


nn en fe ee ee ee ee ee ee ee les 


float compassClass: :getHeading() 
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float tempHeading; 
Boolean checkSumFlag; 
compData rawMessage; 


if ((port2.headings.Get (rawMessage)) && (checkSumCheck(rawMessage))) { 


tempHeading = parseCompData(rawMessage, 'C') * degToRad; - 
currentHeading = continousHeading (trueHeading (tempHeading) ); 


return currentHeading; 


} 
else { 
return currentHeading; // No updated position is available. 
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NAME : asciiToHex 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Administrative conversion function 
RETURNS : Hex version of an ascii character 
CALLED BY: checkSumCheck 

CALLS: None 
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BYTE asciiToHex (BYTE letter) 
{ 
1f. (letter s= 7a). & 
return (letter - 'A' + 10); 
} 
else { 
return (letter - 48); 
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PROGRAM : checkSumCheck 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Calculates the checksum of the compass message and 
compares it to the indicated checksum of the message. 


RETURNS : TRUE, if the message contains a valid checksum 
CALLED BY: initCompass, getHeading 
CALLS: none 


KKK KARE KEK RRR KERR KKK KKK KEKE KER KKK EKER EK REE EKEKEKEKEREEEKREKKEREK KER EEE E / 
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Boolean compassClass: :checkSumCheck (const compData newMessage) 
{ 

BYTE calChkSum(0) ; 

BYTE mesChkSum(0); 


for (int i = 1; newMessage[i] != '*'; i++) { 


calChkSum *= newMessage[i]; 


my 


mesChkSum = asciiToHex(newMessage[i+i]) * 16 
+ asciiToHex (newMessage[i+2]); 


return Boolean(calChkSum == mesChkSum) ; 
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PROGRAM: trueHeading 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Converts magnetic direction to true based on local 
magnetic variation. 


RETURNS : true heading 
CALLED BY: insPosit 

insSetUp 
CALLS : none 
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float compassClass: :trueHeading (const float magHeading) 


{ 
static double twoPi(2.0 * M_PI); 
double trueHeading = magHeading + RADIANMAGVAR; 


if (trueHeading > twoPi) { 


trueHeading -= twoPi; 


} 


return trueHeading; 
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PROGRAM: continousHeading 

AUTHOR: Eric Bachmann 

DATE: 11 July 1995 

FUNCTION: Maintains track of branch cuts & returns a continous heading. 
RETURNS : continous true heading 

CALLED BY: insPosit, insSetUp 

CALLS: none 
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float compassClass::continousHeading(const float trueHeading) 
{ 

const float twoPi(2.0 * M_PI); 

static int branchCutCount (0) ; 

static float previousHeading (trueHeading) ; 


if ((4.71 < previousHeading) && (trueHeading < 1.57)){ 


++branchCutCount; // Went through North in a right hand turn 
} Z 
else { 
if ((1.57 > previousHeading) && (trueHeading > 4.71)) { 
--branchCutCount ; // Went through North in a left hand turn 
} 
} 


previousHeading = trueHeading; 


return trueHeading + (branchCutCount * twoPi); 


LRELELRALEL LEAL ELAS PEELE EA ELE REEL LAE ERE ERE ELLE ELE EAE A TS AREAS Se 


PROGRAM: parseCompData 


AUTHOR: Eric Bachmann 

DATE: 11 July 1995 

FUNCTION: Parses the heading out of a compass message. 
RETURNS : the message heading as a float 

CALLED BY: insPosit, insSetUp 

CALLS: none 
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float compassClass::parseCompData(const compData rawMessage, 
const BYTE key) 


float dataSum(0); 


for(int j = 0; rawMessage[j] != key; j++) (} 
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j++; 
for(int i = 0; rawMessage[i + j] != ‘.'; i++) {} 
switch (1) { 
case 3: 
dataSum = (rawMessage[j] - 48) * 100.0 + 
(rawMessage[j+1] - 48) * 10.0 + 


(rawMessage[j+2] - 48) + (rawMessage[j+4] - 48) * 0.1; 
break; 


case 2: 


dataSum = (rawMessage[j] - 48) * 10.0 + 
(rawMessage[j+1] - 48) + (rawMessage[j+3] - 48) * 0.1; 
break; 


case l: 
dataSum = (rawMessage[j] - 48) + (rawMessage[j+2] - 48) * 0.1; 


break; 


} 


return dataSum; 


} 
// end of file compass.cpp 





O. A2D.CFG 

8 ;seqent :number_of_seq_addresses_to_load 

0 smode_sel:__ DIFF=1__ SE=0 

1 smode_acdc:_Signal_coupling_select__DC=1__AC=0 

8 schent: Number _of_channels_to_sequence_(hex,_1-F) 
3125 ;delta_t:__Sample_rate_in_microsecs_3-8192 

i, ;samprate:__Sample_rate_in_recurrent_mode__0 (fast) -7 (slow) 
0 ;sampindex:_Which_channel_to_sample_in_recurrent_mode 
0 0 0 0 

1 1 0 0 

2 2 0 0 

3 3 0 0 

4 4 0 0 

2 5 0 0 

6 6 0 0 

7 i 0 0 

8 8 0 0 

9 A 2 0 
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A 5 2 0 

B A 2 0 

c 5 2 0 

D A 2 0 

E 5 2 0 

F A 2 0 

P. A2D.H 

#ifndef _A2D_H 

#define _A2D_ H 

#include <dos.h> 

#include <math.h> 

#include <conio.h> 

#include <stdio.h> 

#include <stdlib.h> 

#include <stdarg.h> 

#include <iostream.h> 

#include <fstream.h> 

//ESP A2D General Global Definitions 

#define DEFBASE 0x100 // Base address SEL=1->0x300 & SEL=0->0x100 
#define FIFOSIZE 1000 // FIFO size (MAX=1000 decimal) 

#define MAXCHAN 0x10 // Max channels 

//ESP A2D Status Register Definitions 

//BASE+02h: O11D DDDD 

#define INT_STAT 0x10 // 0001 0000 INTERRUPT STATUS (1=IRQ Pending) 
#define TRG_STAT 0x08 // 0000 1000 TRIGGER STATUS (1=Triggered) 
#define FULL 0x01 // 0000 0001 FIFO FULL (001=Full) 
#define HALF 0x05 // 0000 0101 FIFO HALF FULL (101=Half Full) 
#define EMPTY 0x06 // 0000 0110 FIFO EMPTY (110=Empty ) 
//ESP A2D Control Register Definitions 

//BASE+08h: DDDD DDDD 

//BASE+09h: DDDD DDRR 

#define GATE1LOUT 0x0008 // 0000 0000 0000 1000 GATE1OUT (Always Driven) 
#define TRG_POS 0x0010 // 90000 0000 0001 0000 TRIG POS (Trig on +!-) 
#define SET_TRG 0x0020 // 0000 0000 0010 0000 TRIG SET (Active LOW) 
#define RST TRG 0x0040 // 0000 0000 0100 0000 TRIG CLR (Active LOW) 
#define INT_EN 0x0080 // 0000 0000 1000 0000 IRQ ENAB (Active HIGH) 
#define DIFF 0x0400 // 0000 0100 0000 0000 DIFF/SE (1=DIFF O0=SE) 
#define RMS 0x0800 // 0000 1000 0000 0000 RMS Mode (1=ON O=-OFF') 
#define CAL Oxi1006 // 0001 0000 0000 0000 CAL Mode (1=0ON £O=O0FF) 
#define PRG_SEO O0x1000 // 0001 0000 0000 0000 SEQ Mode (1=PRG OQ=RUN) 
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#define ACDC 0x2000. // 0010 0000 0000 0000 ACDC Mode (1=DC O=AC) 
#define SAM_SEOQ 0x4000 // 0100 0000 0000 0000 SAMP/SEQ (1=SEQ O=SAMP) 
#define RST_FIFO 0x8000 // 1000 0000 0000 0000 FIFO Reset (1=EN O=REW) 


//ESP A2D Useful Definitions 
#define CLRRATE OxFFF8 // CLEAR RATE TO HIGHEST RATE 


//Class- Definition for the A2D Class 
class a2dClass { 


public: 


a2dClass(); // reads a2d.cfg file, initializes hardware 
~a2dClass() { lockTrigger(); } 


void readConfigFile(); // reads a2d.cfg file 

void initA2d(); // initializes the a2d 

void reinitA2d(); // reinitializes the a2d after FIFO overflow 
void initSysAddr (void) ; // sets address mapping 

void initHardware (void); // initializes the a2d control register 


// Print out the variable ctrilw, for debug purposes 
void printCtrilw(void) ; 


// Sets the A2D Control Register for Single-Ended mode 
void setSe(void); 


// Sets the A2D Control Register for Differential mode 
void setDiff (void); 


// loads sequencer memory with channel data 
void setChannel (unsigned seq,unsigned ch,unsigned g10,unsigned g2); 


// Sets sequencer to program mode 
void setProgSeq(void) ; 


// Sets sequencer to run mode 
void setRunSeq(void) ; 


// Loads sequencer address counter with number of channels to scan. 
void setCount (unsigned nch) ; 


void setAcDc(unsigned acdc); // sets AC or DC coupling 
void lockTrigger (void) ; // prevents triggering 
void unlockTrigger (void) ; // allows the trigger to function 
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// Toggle the trigger.(software triggering) 
void setTrigger (void); 


void resetTrigger (void); // clears the trigger 


// Switches in the RMS measurement chip 
void setRmsOn (void) ; 


// Switches out RMS measurement chip 
void setRmsOff (void); 


// Sets the A2D module to sequencer mode 
void setSequencer (void); 


// Sets the A2D module to sampler mode 
void setSamplerRate (unsigned) ; 


// Set GATE1LOUT bit of control word high 
void gateloutOn (void) ; 


// Set GATELOUT bit of control word low 
void gateloutoOff (void); 


// Sets timer channel 1 to square-wave input 
void squareWaveTimeri (unsigned) ; 


// Initialize the A2D timing using timer 2 
void initTiming(unsigned dt); 


void resetFifo(void) ; // rewind FIFO to beginning of memory 
void setFifo(void); // enable FIFO to acquire data 
unsigned getFifoStatus (void); // returns the state of the FIFO 


// Returns next data word stored in FIFO 
signed getFifoData (void) ; 


// Program timer channel 0 to set the desired interrupt rate 
void setIntRate(unsigned intrate); 


void intOff (void); // locks out the interrupt request line 
void intOn(void); // enables system interrupt request 


// Sets the trigger level; trigger level (0=-10V, 128=0V, 255=+10V) 
void setTriggerLevel (unsigned tl); 


// Sets falling or rising edge trigger 
void setTriggerPosition(unsigned tp); 
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void zeroOffset (void); . // calibrates zero offset error 





// Grounds the two differential inputs for zero adjust 
void grndinput (void) ; 


void freeInput (void) ; // ungrounds the two differential inputs 


void: zeroAdjust (void) ; // adjust the trimmer on the PGA 


// Number of channels to sequence 
// period between channels 


int chent; 
unsigned delta_t; 


private: 


// Holds A2D Control Register update values 
// Sequence Counter 
// Single-ended or Differential 
// AC/DC Coupling 
// Sample Rate in Recurrent Mode 

// Which Channel to Sample in Recurrent Mode 
// Sequencer Address ; 


unsigned ctrlw; 

unsigned seqcnt; 

unsigned mode_sel; 
unsigned mode_acdc; 
unsigned samprate; 
unsigned sampindex; 
unsigned seqaddr [MAXCHAN] ; 


unsigned chan[MAXCHAN] ; // Channel 

unsigned g10[MAXCHAN] ; // x10 Gain 

unsigned g2[MAXCHAN] ; // x2 Gain 
d; 
#endif 
Q. A2D.CPP 
#include "a2d.h" 
//ESP A2D Addresses 
unsigned BASE - DEFBASE; // BASE I/O ADDR [BASE] () 
unsigned FIFO = 0x00; // FIFO READ ADDR [00-01] (R) 
unsigned MEM = 0x00; // SEQUENCER ADDR fOO-01] (WwW) 
unsigned STAT = 0x02; // STATUS REGISTER [02] (R) 
unsigned COUNT = 0x02; // SEQUENCER ADDR PTR [02] (W) 
unsigned TIMERO = 0x04; // TIMER 0 [04] (R/W) 
unsigned TIMER1 = 0x05; // TIMER 1 [05] (R/W) 
unsigned TIMER2 = 0x06; // TIMER 2 [06] (R/W) 
unsigned TIMERC = 0x07; // TIMER CONTROL WORD {O07 ] (R/W) 
unsigned CNTL = 0x08; // A2D CONTROL REGISTER [08-09] (W) 
unsigned DAC = 0x0C; // DAC DATA [OC] (W) 
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// FUNCTION NAME: a2dClass() 

// AUTHOR: Randy Walker 

// DATE: 27 March 1996 

// DESCRIPTION: Reads a2d.cfg file, initializes address map and hardware 
// RETURNS: void 

// CALLS: readConfigFile(), initSysAddr(), initHardware() 

// CALLED BY: Object declaration 

// 


Se Fe Se KK BK KKK KK KK KK KKK KKK KI KK KKK KKK KKK KK EKER KEK EREKEKKEKEREKKRKERREKKKKEKEEE 


a2dClass::a2dClass (void) 
{ 


cerr << "constructing a2d1" << endl; 


ctrlw=0; 
seqcent=1; 
mode_sel=0; 
mode_acdc=1; 
delta_t=3; 
chent=1; 
samprate=0; 
sampindex=0; 
readConfigFile(); 
initSysAddr (); 
initHardware(); 


Ee en en eR ere nen rrr rr a Tener 
// FUNCTION NAME: readConfigFile() 

// AUTHOR: Randy Walker, based on [MAXKUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Reads the a2d.cfg file and sets variables 

// RETURNS: void 

// CALLS: none 

// CALLED BY: a2d class constructor 

// 
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void a2dClass::readConfigFile({) 
{ 

FILE *configFile; 

char junk[{128]; 


if ((configFile = fopen("a2d.cfg", “r")) == NULL) { 
fprintf(stderr, “Cannot open file A2D.CFG...\n"); 
exit (1); 

} 
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fscanf (configFile,"$x%s",&seqcent, junk) ; 


if (seqent==0 || seqent>0x0F){ // seqent must be 1-F (15 max in seq mode) 
cout << "\nseqent out of range in A2D.CFG...\n"; 
exit(1); 

} 

fscanf (configFile, "$d%s",&mode_sel, junk) ; 

if (mode_sel !=0 && mode_sel != 1){ 
cout << "\nmode_sel out of range in A2D.CFG...\n"; 
exit(1); 

} 

fscanf (configFile, "%d%s",&mode_acdc, junk) ; 

if (mode_acde !=0 && mode_acde != 1){ 
cout << "\nmode_acde out of range in A2D.CFG...\n"; 
exit(1); 


} 


fscanf (configFile, "%x%s",&chent, junk) ; 
if (chent == 0 || chent > Ox0F){ //chent must be 1-F (15 max in seg mode) 
cout << "\nchent out of range in A2D.CFG...\n"; 





exit(1); 

} 

fscanf (configFile, "%d%s",&delta_t, junk) ; 

if (delta_t < 3 {| delta_t > 8192) { 
cout << "\ndelta_t out of range in A2D.CFG...\n"; 
exit(l1); 


if (delta_t < 6 && chent > 1){ 
cout << "\ndelta_t must be > 6 for chent > 1..-.\n"; 


exit(l1); 
} 


fscanf (configFile, "$d%s",&samprate, junk) ; 
if (samprate > 7) { 
cout << "\nsamprate out of range in A2D.CFG...\n"; 


exit (1); 
} 


fscanf (configFile, "%x%s", &sampindex, junk) ; 
if (sampindex > Ox0F) { 
cout << "\nsampindex out of range in A2D.CFG...\n"; 


exit(1); 
} 
for (int i = 0; i < segent; i++) { 

fscanf (configFile, "%x%x%x%x", &seqaddr[i],&chan{i],&g10[i],&g2[i]); 
} 


fclose(configFile) ; 
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// FUNCTION NAME: initSysAddr () 

// BUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Sets system address mappings 

// RETURNS: void 

// CALLS: none 

// CALLED BY: a2d class constructor 
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void a2dClass::initSysAddr (void) 


{ 
//clear BASE 


FIFO &= OxOF; // FIFO READ ADDRESS [00,01] (R) 
MEM &= Ox0F; // SEQENCER MEM ADDRESS [00,01] (W) 
STAT &= Ox0F; // STATUS REGISTER [02] (R) 
COUNT &= OxOF; // SEOENCER ADDRESS PTR [02] (W) 
TIMERO &= OxOF; // TIMER 0 [04] (R/W) 
TIMERI &= Ox0F; // TIMER 1 [05] (R/W) 
TIMER2 &= Ox0F; // TIMER 2 [06] (R/W) 
TIMERC &= OxO0F; // TIMER CONTROL WORD [07] (R/W) 
CNTL &= Ox0F; // CONTROL REGISTER [08] (R/W) 
DAC &= Ox0F; // DAC DATA [0c] (W) 
//set BASE 

FIFO [= BASE; // FIFO READ ADDRESS [00,01] (R) 
MEM |= BASE; // SEQENCER MEM ADDRESS [00,01] (W) 
STAT |= BASE; // STATUS REGISTER [02] (R) 
COUNT |= BASE; // SEQENCER ADDRESS PTR [02] (W) 
TIMERO |= BASE; // TIMER 0 [04] (R/W) 
TIMER1 |= BASE; // TIMER 1 [05] (R/W) 
TIMER2 |= BASE; // TIMER 2 [06] (R/W) 
TIMERC [= BASE; // TIMER CONTROL WORD [07] (R/W) 
CNTL |= BASE; // CONTROL REGISTER [08] (R/W) 
DAC |= BASE; // DAC DATA [OC] (W) 
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// FUNCTION NAME: initA2d() 

// AUTHOR: Rick Roberts 

// DATE: 13 November 1996 

// DESCRIPTION: Performs necessary steps for initialization of the a2d 
// or to reinitialize if acceleration parameters are in 
ae error due to a poor initial data transfer. 

// RETURNS: void 

// CALLS: setRmsOff(), setSequencer(), lockTrigger(), resetFifo(), 

// unlockTrigger(), and setTrigger(), all in a2d.cpp 

// CALLED BY: sampler class constructor 
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void a2dClass: :initA2d(void) 
{ 
setRmsOff(); 
setSequencer(); 
lockTrigger (); 
resetFifo(); 
setFifo(); 
unlockTrigger(); 
setTrigger(); 
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// FUNCTION NAME: reinitA2d() 

// AUTHOR: Rick Roberts 

// DATE: 13 November 1996 

// DESCRIPTION: Performs necessary steps for reinitialization of the a2d 
// or to reinitialize if acceleration parameters are in 
// error due to a poor initial data transfer. 

// RETURNS: void 

// CALLS: readConfigFile(), initSysAddr(), initHardware(),2 — 

// setRmsOff(), setSequencer(), lockTrigger(), resetFifo(), 
// unlockTrigger(), and setTrigger(), all in a2d.cpp 

// CALLED BY: sampler class readSamples if a2d FIFO has overflowed 
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void a2dClass: :reinitA2d(void) 

{ 
readcConfigFile(); 
initSysAddr (); 
initHardware(); 
setRmsOff(); 
setSequencer (); 
lockTrigger(); 
resetFifo(); 
setFifo(); 
unlockTrigger(); 
setTrigger(); 





} 
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// FUNCTION NAME: initHardware() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Sets the A2D Control Register to 0020 and sets the data 
Py member, ctrlw=0060; initializes the module setup for 

// software triggering of the A2D. Programs each channel. 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: a2d class constructor 


L[ESEELERERAEELELELE SE ERRER EAE LER AEE ELA EE RARER REEL ELLE SELES AREER SS 





138 








void a2dClass: :initHardware (void) 


{ 


outpw(CNTL, SET_TRG) ; 
ctrlw = SET_TRGIRST_TRG; 


if (mode_sel == 0) 
setSe(); 


else 
setDiff(); 


for (ine i=: 0s1.-< chentsi++)4 
setChannel (segaddr[{ij],chan[{i],g1l10[i],g2[1i]); 
} 
setAcDe (mode_acdc) ; 
initTiming (delta_t) ; 
setCount (chent); 


LLELELESEELEREL ELLA LSE E ALE LA LEER ERAS ERA ARELE REALS LAE SE RES SLES OD 


Ee 


// 
// 


FUNCTION NAME: printCtriw() 

AUTHOR: Randy Walker, based on [MAXUS 95] code 

DATE: 27 March 1996 

DESCRIPTION: Print A2D control register var, ctrlw. 
The variable is used to set a byte in the 
ESP A2D control register at BASE + 08h/09h 
Used during application code debug 

RETURNS: void 

CALLS: none 

CALLED BY: none 


ff EEEELESLELERER ESLER ELE LELAEL ELAR ELE AEA L EAE ERE A LS SAAS ES ORS 


void a2dClass::printCtrlw(void) 


{ 


printf£("ctrlw: %04x\t", ctriw); 
for (int i=0x00; i < 0x10; i++) { 
printf ("Si", ((ctrlw>>0x0F-1) & 1)); 
if ((1+1)%4==0) 
Printec™ 7); 
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// FUNCTION NAME: setSe() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Sets ctrlw for single ended mode and writes ctriw to 
// A2D Control Register 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: initHardware () 
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void a2dClass::setSe (void) 
{ 
ctrlw &= ~DIFF; 
outpw(CNTL, ctriw) ; 
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// FUNCTION NAME: setDiff() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Sets ctriw for differential mode and writes ctrlw to 
// A2D Control Register 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: initHardware() 
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void a2dClass::setDiff (void) 
{ 
ctrlw |= DIFF; 
outpw(CNTL, ctrlw) ; 
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// FUNCTION NAME: setChannel () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Loads sequencer memory with channel data 
// CALLS: progSeq(), outpw(), runSeq() 

// CALLED BY: initHardware() 

// VARIABLES: seg - sequencer number 


If ch - channel number 
es gi0 - x10 gain value 
es g2 - x2 gain value 
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void a2dClass::setChannel (unsigned seg,unsigned ch,unsigned gl10, 
unsigned g2) 


{ 
unsigned d = 0; 
setProgSeq(); // set sequencer program mode 
outpw (COUNT, seq) ; // set sequencer address 
//load sequencer memory 
dad |= ch<<8; // channel 
d l= (g2<<12); // gain X2 
d l= (g10<<14) ; // gain X10 
outpw (MEM, d) ; // load sequencer 
setRunSeq(); // set sequencer run mode 
} 
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// FUNCTION NAME: setProgSeg() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Sets sequencer to program mode 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: setChannel() 
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void a2dClass: :setProgSeq (void) 
{ 
ctrliw |= PRG_SEQ; 
outpw(CNTL, ctriw) ; 
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// FUNCTION NAME: setRunSeq() 

// BRUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Sets sequencer to run mode 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


f [EELESELLEAESESERELA LARA RERAEERAERERAERALEE ELE SEERA ERE L ERE AR SR 


void a2dClass::setRunSeg (void) 


{ 
ctrlw &= ~PRG_SEQ; 
outpw(CNTL, ctrlw) ; 


14] 








[[REEEEREEELER EER ERE REALE EE A EEELAL ELS EREA EERE ELEEE LER EE LER SE SESE ENO 


if FUNCTION NAME: setCount () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Loads sequencer address counter with number of channels 
// to scan. 

// RETURNS: vo: 

// CALLS: outpw(), setProgSeq(), setRunSeq() 

// CALLED BY: initHardware() 

// VARIABLES: nch - number of channels to sequence 


L[BERRREEEARERER SE EERE EEE RE REE EERE EK E HEHE EE EE LEER EERE REEE LESSER ERS 


void a2dClass::setCount (unsigned nch) 


{ 
nch=nch<<4; // put in upper nibble 
outpw (COUNT, nch) ; // out to register 
set ProgSeq(); // reset sequencer 
setRunSeg(); // put it in run mode 
} 


J [BERR RR RE REE EER REE EK ERE EEE RE KIRK HH EEE EEE EE ERE REL ERE ERE BERS S 


// FUNCTION NAME: setAcDc() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Sets AC or DC Coupling 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: initHardware() 

// VARIABLES: acde - holds coupling value 


[ [BERR RRERERERERE ER EKER ERE EEE EE EER ER ERR ERE ERE REE E EEE EEE REREEER ELE S 


void a2dClass::setAcDc (unsigned acdc) 


{ 


if (acdc) 

etriw |=-ACDC; // acde=1 -> DC 
else 

ectrlw &= ~ACDC; // acdc=0Q -> AC 


outpw(CNTL, ctrlw) ; 


[ [BERR E RE EEEERIER ERE E EERE EEEEEE HR ERER EHH EERIE ERES EEE ER EE EERE EE ELLE 


// FUNCTION NAME: lockTrigger () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Prevents triggering 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main ? 


[[RELLLELEELELL ELE SRE SALA ER EREL ERE OES EES LALAL AR ERE LS RENAE A EEE RARE AOE 
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vold a2dClass::lockTrigger (void) 
{ 
ctrlw &= ~RST_TRG; 
outpw(CNTL, ctrlw) ; 


[PREPRESS LEEAELE AAS EAE EERE SSAA EERE AEE SEAR ERA ESR ES RR ee Oe ee ean 


// FUNCTION NAME: unlockTrigger () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Allow the triger to function 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


[ [RE REELEERE REESE REE REREA ELLER EEE EEE ERER EERE REEL EE EERES ERE RARE ER ELE EE 


void a2dClass: :unlockTrigger (void) 
{ 
ctrlw |= RST_TRG|SET_TRG; 
outpw(CNTL, ctrlw) ; 


f [RERERALEALES LEAR LER ESA LEELA SD LEELA LA EA ALE EERE ELEN TEE RE EEE SE 


// FUNCTION NAME: setTrigger () 

// RUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Toggle the trigger (software triggering) 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


Lf REEEEELE NEEL EERE LEER ELA LEER ARLES ESAS AEE LES ELA SE RSS NER 


void a2dClass::setTrigger (void) 

{ 
outpw(CNTL, ctrlw&~SET_TRG|RST_TRG) ; 
outpw(CNTL,ctrlw| SET_TRG|RST_TRG) ; 


[JL FLEES EIREELELELEAE LESS LSA ELE LS LEE LER EA ARLES EES ERE Be ee ee ee 


// FUNCTION NAME: resetTrigger () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Clears the trigger 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


LL EREESAEAELE LEAER SEL AES LEA RELEASE EAS SAE ER LAE AALS LEA SAS Se eR 
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void a2dClass: :resetTrigger (void) 

{ 
outpw(CNTL, ctrlw!|SET_TRG&~RST_TRG) ; 
outpw(CNTL, ctrlw|SET_TRG| RST_TRG) ; 


[LEEEERLESRESLEREREREEELL ELAS SERS ESE LEELA RELL AEA EE EAR CeCe eS ne 


// FUNCTION NAME: setRmsOn() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Switches in the RMS measurement chip 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


LLEEEEELE ELE EEA ERS LE PRAT ARALEL ERLE RA ARE EEEA LEAL ERE AAL LEAL EAE A SRE EES 


void a2dClass::setRmsOn (void) 
{ 

ctrlw [|= RMS; 

outpw(CNTL, ctriw) ; 


[ [ERE EELELAREAREEEE EEE AE SELES EE EARL A REL AERA LE LEL LE LASALLE EE EEE RANG 


// FUNCTION NAME: setRmsOff () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Switches out RMS measurement chip 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


J LEEPER AER SE REAR LLL A EERE ER ELESA ERE REALE ALE LEL ESS ELAS AAS ES SS 


void a2dClass::setRmsOff (void) 
{ 

ctrilw &= ~RMS; 

outpw(CNTL, ctrlw) ; 


LL£EEELEELALEE EAE EERE EL EES LAE ESL LEER AAAS RES RAS SP RIL LA ee eo 


// FUNCTION NAME: setSequencer () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Sets the A2D module to sequencer mode 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


J [RELELELEREERLLELER LEER ELLER LEE ERAL ELLER EE ASS SEE EAE ERA EES ERAS 
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void a2dClass::setSequencer (void) 


{ 


ctrlw |= SAM_SEO; 
outpw(CNTL, ctrlw) ; 


LLELEEAEELELEEELLERLE RARER ELEC E AEE REAE EARLE REALE AAR SEES SR ay Be ee hoe 


if 
// 


es 


FUNCTION NAME: setSamplerRate() 

AUTHOR: Randy Walker, based on [MAXUS 95] code 
DATE: 27 March 1996 | 

DESCRIPTION: Sets the A2D module to sampler mode 
RETURNS: void 

CALLS: outpw() 

CALLED BY: main 

VARIABLES: rate - sampler rate 


Lf PREREEEAA AEE REEL ERL ERA LE ELAS A EAS EEA ESE EELS SE EDL SC ee ee 


void a2dClass::setSamplerRate (unsigned rate) 


{ 


ctrlw &= ~SAM_SEQ; //Set to sampler mode 

ctrlw &= CLRRATE; //Clear previous rate to 000 
ctrlw |= rate; //Set new rate 

outpw(CNTL, ctrlw) ; //Set Control Word 


LLEEEEEREEEEE RARER SELA EEA ARER EL EL ASE SELES AL A REL A ee eS ee ee 


// 


// 


FUNCTION NAME: gateloutoOn() 

AUTHOR: Randy Walker, based on [MAXUS 95] code 
DATE: 27 March 1996 

DESCRIPTION: Set GATE1OUT bit of control word high 
RETURNS: void 

CALLS: outpw() 

CALLED BY: main 


[[REEEEEELEELER ELA EEA AERA EA SELES ES SAREE EL EALELA LAS LAER EES LS ee eee 


void a2dClass: :gateloutOn (void) 


{ 


ctrlw {|= GATEI1OUT; 
outpw(CNTL, ctrilw) ; 
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// 
es 
iy 
// 
id. 
// 
if 


FUNCTION NAME: gateloutoOff () 

AUTHOR: Randy Walker, based on {[MAXUS 95] code 
DATE: 27 March 1996 

DESCRIPTION: Set GATE1OUT bit of control word low 
RETURNS: void 

CALLS: outpw() 

CALLED BY: main 


LLRELERELEA ALE EERE RES EA AEE AES AREA RAL EL NR ene ae 


void a2dClass: :gateloutOff (void) 


{ 


ctrlw &= ~GATE1OUT; 
outpw(CNTL, ctrlw) ; 


f [ZEEE EELIER AE LEELA EEL ASE ERE LAS EAL EL LEA NO ee Noe ee nee ne ree 


fy 
// 
fy 


FUNCTION NAME: squareWaveTimerl! () 
AUTHOR: Randy Walker, based on [MAXUS 95] code 
DATE: 27 March 1996 
DESCRIPTION: Sets timer channel 1 to square-wave input 
RETURNS: void 
CALLS: outp() 
CALLED BY: main 
VARIABLES: dt-micro seconds per period (1 to 8192) 
assuming 8 MHz ciock input 
ch-timer channel 1 
ph-local variable 
pl-local variable 


[[REEEEAAA ERE EAES ERLE AALELE LEE ELA EE LES Ee Re ee ee en ee eee re 


void a2dClass: :squareWaveTimer1 (unsigned dt ) 


{ 


char ph,pl; 


one (dt*8) &0OxFF; // 8 CLOCKS PER uS 
ph = (dt*8)>>8; 


outp (TIMERC, 0x76) ; // initialize timer 
outp (TIMERI,p1) ; // at uS delay 
outp (TIMER1, ph) ; // with 8 MHz clock 
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// FUNCTION NAME: initTiming() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Initialize the A2D timing using timer 2 
// RETURNS: void 

// CALLS: outp() 

// CALLED BY: initHardware () 

// VARIABLES: dt - number of micro seconds (3 to 2730) 


LL ALEEARE EERE ELAS LES ERA AAARALEA LE LARA ERA ARDS ER NR ee Rr 


void a2dClass::initTiming(unsigned dat) 


{ 
char jsjoure on 
pl = ({dt"s) c0xek; // 8 CLOCKS PER uS 
ph = (dt*8)>>8; 
outp (TIMERC, 0xB6) ; // initialize timer2 
outp (TIMER2,p1) ; // dt uS delay 
outp (TIMER2, ph) ; // with 8 MHz clock 
} 


Lf PEELE AALS ECLA AEE ERA LARA RSET PERS Ae ES A ee eee em een een ee 


// FUNCTION NAME: resetFifo() 

// AUTHOR: Randy Walker, based on [MAKUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Rewind FIFO to beginning of memory 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


LJLELERERELER ES LEER RELEASE SELLA LALA LE SELL LES EERE RRS SS ee 


void aZdClass::resetFifo(void) 
{ 
ctrlw &= ~RST_FIFO; 
outpw(CNTL, ctrlw) ; 


[Lf RELEEEESEERREL SLA LES EELS RSA SS EERE RDS RR ERR RS IMS Oe Ne Ree Nee oe 


// FUNCTION NAME: setFifo() 

// BRUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Enable FIFO to acquire data 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


[LRRERERELALELEA ERE EELER ERE LE LE LAL EER LEE ERE EREAEEERLA LALA REESE RENEE 
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void a2dClass::setFifo(void) - 
{ 
ctrlw |= RST_FIFO; 
outpw(CNTL, ctrlw) ; 


LAER EIA ERE RELAE EAL EEA REL ASA ALE RS Ne ve PA ee ee ne 


// FUNCTION NAME: getFifoStatus () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996. 

// DESCRIPTION: Returns FIFO status 

// RETURNS: RETURNS: 6 - empty 

Ly 5S - half full 

es 1 - full 

// CALLS: inpw() 

// CALLED BY: main 


Lf RAELERELLE EA ALLELES ERASE AEE LAPSES SEALS ALS EESLLL SD LER ER SLANT PE OO 


unsigned a2dClass: :getFifoStatus (void) 


{ 
return (inpw(STAT) &7); 


LLFREREELS RELL EEE EEE LARA LES LEELEL EE SERA LEREL ES SEALS AES LES SS RN 


// FUNCTION NAME: getFifoData() 

// BUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Returns next data word stored in FIFO 
// RETURNS: 16bits of data. Lower 12 are A2D data 

// CALLS: inpw() 

// CALLED BY: a2da class constructor 


[LEELA EEERERE LAE LEL AAT LE SEA AEER EAA EELS ELE SEES EEL AAS ERE ERS, 


signed a2dClass::getFifoData (void) 


{ 
return (inpw(FIFO)&0x0FFF) ; //Get data and mask upper nibble 


} 


I J[EEEELELEELARELA LE REL ELLE LEER ALES ALE AAA EELS LE ELON BREEN RO ee 


// FUNCTION NAME: setIntRate() 

// RUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Program timer channel 0 to set the desired interrupt rate 
// RETURNS: void 

// CALLS: outp() 

// CALLED BY: main 

// VARIABLES: intrate-micro secs per period (1 to 8192) 


i} assuming 8 MHz clock input 
f [RRR ER EEE ELRENIELE RA EEE ELAE ER ELLER ER LERE REE EE EREE REEL EEE A ALES EEE ASE 
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void a2dClass::setIntRate(unsigned intrate) 


{ 
outp (TIMERC, 0x36) ; // Set timer 0 to mode 3 
outp(TIMERO, (intrate*8)&0xFF); // Load Least Significant Byte 
outp (TIMERO, (intrate*8)>>8); // Load Most Significant Byte 


LI EEELAERLEAE LELEAEEL EAL ES ERES EELS AP AR AER ES RES A Be Pe men een ee nie i ee 


// FUNCTION NAME: intoOfft() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Locksout the interupt request line 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


TL {[ERERAEAELELEL SE EPEELERALAL RELA AS EAL AE SLA LRA SEAS RRR RO So ee ee 


void a2dClass::intoOff (void) 

{ 
ctrlw &= ~INT_EN; // INT_EN is active high 
outpw(CNTL, ctrlw) ; 


LJ EEEEELERELSE LEELA EREL EER ER AERA EEE AREER SLE SEE ELE ES NS Se ee ee a ee ee 


// FUNCTION NAME: intOn() 

// RUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Enables system interuppt request 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


[LEE EELEREEEAELEAREEL EL ELSE LEE E LAR RERERK EE RARE RAL EER ELAR EAS BO eer 


void a2dClass::intOn(void) 

{ 
ctrlw |= INT_EN; // INT_EN is active high 
outpw(CNTL, ctrlw) ; 

} 


[LEESEEE AS EELS LEER ELDERS SAAS RELA EAR AEA ERAS SE ee en ee Re 


// FUNCTION NAME: setTriggerLevel () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 | 

// DESCRIPTION: Sets the trigger level 

// RETURNS: void 

// CALLS: outp() 

// CALLED BY: main 

// VARIABLES: tl-trigger level (0=-10V, 128=0V, 255=+10V) 


[LLEELEEEELELEL ELLER LEELA SLL ELLE ELAR EERE LES SESE EEE AES EAA LA ESR ERA LES 
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void a2dClass::setTriggerLevel (unsigned tl) 
{ 

outp (DAC, t1) ; 
} 


J [EERE EEELELE LER EALELEL LEELA ALA ALE LELEL ER LEAR CREE AE AEA EARN ee 


// FUNCTION NAME: setTriggerPosition () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Sets falling or rising edge trigger 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 

// VARIABLES: tp: O=falling, l=rising 


ff ELEELELA EEL ALLELE LEELA ELL ER ELLA LEELA EA ELAS EES ELAR EAS REESE ONS eS ee AR 


void a2dClass::setTriggerPosition(unsigned tp) 


{ 
ctrlw &= ~TRG_POS; //Clear previous TRG_POS 
ctrlw |= (tp) ?TRG_POS:0; //Evaluate tp and set ctrlw 
outp(CNTL, ctrlw) ; 

} 


LJ ELREEEEELEES ERE A EAL EAR RAS SS SRS OR RR NE eT A a ne ae ee tee 


// FUNCTION NAME: zeroOffset() 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Calibrates zero offset error 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: a2d class constructor 


[L[ PREELEEELELL ESE EASA LEER AEE RAE AEL AS ES ELLE ELLE SE EOL Se A Meh ee eee 


void a2dClass::zero0Offset (void) 


{ 
unsigned d=0,1,g2,g10; 
float sum; 
float offsetErr[{4] [4]; 


float bits[4] [4]; 
unsigned gainsl10[4] = {1,10,100,100}; 
unsigned gains2[4] = {1, 2, 4, 8}; 


elrsecr(): 
printf ("\n\tG10\tG2\t OFFSET\t\t BITS"); 


for(gl0 = 0; gl10 < 4; gl10++) 
for(g2 = 0; g2 < 4; g2++) 
printf ("\n\tSd\t%d\t+X .XXXXXX\t+XX.X",g10,g2) ; 
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setRmsOff(); 

setAcDc (0); 

setSequencer(); 

initTiming (3); 

setChannel(0,0,g10,92); 

grndinput (); 

delay (5); //Let new gain values stabilize 


while (!kbhit()) { 
for (g10 = 0; gi0 < 4; g10++){ 
for (g2 = 0; g2 < 4; g2++){ 
setChannel (0,0,g10,g2) ; 
grndinput (); 
lockTrigger(); 
resetFifo(); 
setFifo(); 
unlockTrigger(); 
setTrigger(); 
delay (1); 
while (getFifoStatus() != FULL); 
lockTrigger(); 


for (i = 0, sum = 0.0; i < FIFOSIZE; i++) { 
d=getFifoData(); 
sum+= (float) d*i0/2048; 
} 
offsetErr([gl10] [g2]=((float) (sum/FIFOSIZE) -10)-/ 
| (float) (gains10[g10]*gains2[g2]); 
bits[g10][{g2] = 
(float) (offsetErr[g10] [g2]*4096/20*gainsi0[g10]*gains2[g2]}); 
} 
clrscr(); 
printf ("\n\tGloO\tG2\t OFFSET\t\t BITS") ; 
for (g10 = 0; g1i0 < 4; g10++){ 
for (g2 = 0; g2 < 4; g2++){ 
printf ("\n\tSd\tsd\t+1. 6£\t3+04.1£",g10,g2, 
offsetErr[g10][g2],bits[g10][g2]); 


} 


} 
freeInput(); 
getch(); 
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// FUNCTION NAME: grndInput () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 

// DATE: 27 March 1996 

// DESCRIPTION: Grounds the two diff input for zero adjust 
// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


J [ RRERREEREREE REE EERE EE REERER KERR ERE KERR ERE KEELER EER EER ERR KEKE A REESE 


void a2dClass::grndInput (void) 
{ 

ctrlw |= CAL; 

outpw(CNTL, ctrlw) ; 


LJEERELELELRE ELLE LE SALES RE EEE ENE LAER RR CO RM ONE OO OR ee SO RL eS oe 


// FUNCTION NAME: freeInput () 

// AUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Ungrounds the two diff inputs 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


[J BERREREEEEE LEER EL ELE KEELE ERA ERE EEE EERE EER LEER ERELAELRERERL REE RE 


void a2dClass::freeInput (void) 
{ 

ctrlw &= ~CAL; 

outpw(CNTL, ctrlw) ; 
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// FUNCTION NAME: zeroAdjust () 

// BRUTHOR: Randy Walker, based on [MAXUS 95] code 
// DATE: 27 March 1996 

// DESCRIPTION: Adjust the trimmer on the PGA 

// RETURNS: void 

// CALLS: outpw() 

// CALLED BY: main 


[LES REERELEEA LK EA RERALRREREARE SEE AELE ALE SAS REL EL EARLS AS MA ee ee en ee eo 


void a2dClass::zeroAdjust (void) 


{ 
int as 
unsigned d; 
float sum,offsetErr; 


clrser(); 
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printf£("\n\nADJUST THE. TRIM POT FOR 0.0 OFFSET\n\n") ; 


setRmsOff(); 
setAcDc (0); 
setSequencer (); 
initTiming (3); 


while(!kbhit()) { 
setChannel (0,0,3,3); 
grndInput (); 
lockTrigger (); 
resetFifo(); 
setFifo(); 
unlockTrigger(); 
setTrigger(); 
while(getFifoStatus() != FULL); 
lockTrigger(); 


for (i = 0, sum = 0.0; i < FIFOSIZE; i++) { 
d = getFifoData(); 


sum += (fioat)d*10/2048; 
} 


offsetErr=( (float) (sum/FIFOSIZE)-10)/8000.0; 


printf("\tTHE MEASURED DC OFFSET IS: %+8.6f\r",offsetErr) ; 
} 


freeInput (); 


getch(); 


} 


// end of file a2d.cpp 
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APPENDIX B: Serial Port Communications Source Code (C++) 


A. GLOBALS.H 


#ifndef 


#define 


_GLOBALS_H 


_GLOBALS_H 


#include <dos.h> 


// types 


typedef 
typedef 
typedef 


#define 
#define 


unsigned charBYTE; 


unsigned short WORD; 

unsigned long DWORD; 

MEM (seg,ofs) (*((BYTE far*)MK_FP(seg,ofs) )) 
MEMW (seg, ofs) (*((WORD far*)MK_FP(seg,ofs) )) 


enum Boolean {FALSE, TRUE}; 


// basic bit twiddles 


#define 
#define 
#define 
#define 
#define 


set (bit) (1<<bit) 

setb(data,bit) (data | set(bit)) 
clrb(data,bit) (data & !set(bit)) 
setbit (data,bit) (data = setb(data,bit) ) 
clrbit (data, bit) (data = clrb(data,bit) ) 


// specific to ports 


#define 
#define 


setportbit (reg, bit) (outportbh(reg,setb(inportb(reg),bit))) 
clrportbit(reg,bit) (outportb(reg,clrb(inportb(reg),bit))) 


// navigation conversion factors and useful global variables 


#define 
factors 
#define 
#define 


MSECS TO DEGREES (1.0/(1000.0 * 3600.0)) // time conversion 


DEGREES_TO_MSECS 3600000.0 
MINS_TO_MSECS 60000.0 


// Conversion constants for location of 36:35:42.2N and 121:52:28.7W 


#define 
#define 
#define 
#define 
#define 


#define 


#endif 


LatToFt 0.10134 // converts degrees Latitude to ft 
TongToFt 0.08156 // converts degrees Longitude to ft 
HemisphereConversion -1 // -1 if west of of Greenwich 
RADIANMAGVAR 0.261799 // Local Magnetic variation in radians 


radToDeg (180.0/M_PTI) 
degToRad (M_PI/180.0) 
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B. BUFFER.H 


#ifndef _BUFFER_H 
#define _BUFFER_H 


#include "toetypes-.h" 
#include “globals.h" 


#define ONE (unsigned short)1 


FRELEREERE EEE EEA ALEARELLEAAE RAE ERE REEL ELA ERAS ES SEE NO RR ee ee ee DO 


CLASS: bufferClass 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995 

FUNCTION: Base class for use as a polymorphic reference in the 


serial port code which defines a buffer to be used in 
serial port communications. 


eK KI KKK KK KKK KK KKK KK KKK KKK KKK KKK KKK EK KEK KEKE KRKKKRKEKREKKKEKRKKEEKEKKKKKEKKERKEEEK 


class bufferClass { 
public: 
// Constructor 
bufferClass (WORD sz); 
~bufferClass() {} 


// Checks for the arrival of new characters in the buffer 
Boolean hasData() { return Boolean(putPtr != getPtr); } 


// How much of the Buffer is used (rounded percentage 0 - 100) 
int capacityUsed(); 


Boolean Get (BYTES&) ; // vead from the buffer 
void Add(BYTE); // write to the buffer 
protected: 


// Increment the pointer to next position 


void inc(WORD& index) { if (++index == size) index = 0; } 
WORD before(WORD index) // decrement the pointer 
{ return ((index == 0) ? size - ONE : index - ONE) ;} 
WORD getPtr; // Location of unread data 
WORD putPtr; // Location to read data to 
WORD size; // Size of the buffer in bytes 
BYTE* buf; 
7 
#endif 
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C. BUFFER.CPP 


#include <iostream.h> 
#include <stdio.h>: 


#include “globals.h" 
#include "buffer.h" 


LI REEEEELERAEELELSA ESET LEER AS EELS ERAS CHR LES ERLE AN REAL ES Ten 


// FUNCTION NAME: bufferClass constructor 


// AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 

// DATE: 11 July 1995 

// DESCRIPTION: Instantiates a buffer 

// RETURNS: void 

// CALLS: none 

// CALLED BY: compBuffer, GPSbuffer, bufferedSerialPort constructors 
// 


eK KKK TK KK KKK KKK KKK KKK KKK KEE KEE KRRKEKKEEKKKEKKEKEREKRKEKKEKREREREEER 


bufferClass: :bufferClass (WORD sz) : getPtr(0), putPtr(0), size(sz) 


{ 
buf = new BYTE[si1ze]; 


} 


J [RELEEEELES LOREAL EL LAER ELE EE ELE REESE SER ELLER SEALER SEER ST SRL RRS ES 


// FUNCTION NAME: capacityUsed() 


// AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
// DATE: 11 July 1995 

// DESCRIPTION: Returns the rounded percentage of the buffer used. 
// RETURNS: void 

// CALLS: none 

// CALLED BY: bufferedSerialPort::processiInterrupt 

// 


Be IK FE TKK KT KR KK TK IK IK KKK KKK KKK KK KKK KEKE KEKE KEKKKKEKRKKKKKREKEKEEKEEEEK 


int bufferClass: :capacityUsed() 
{ 


int cap = (putPtr + size) % size - getPtr; 
return 100 * cap / size; | 
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Ff PELEREEEERER ERAS LEASE LEAL REE RELEASES EEE EES ELSA LER SEE ES 


// FUNCTION NAME: Get 


// AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
/7 DATE: 11 July 1995 

// DESCRIPTION: Reads a character from the buffer 

// RETURNS: Boolean 

// CALLS: hasData () 

// CALLED BY: GPSbufferClass, compBufferClass 

as 


he te eek RRS IRR KOR I TOTO TOI TIE TOR TR KTR TOR RR RE KK RRR KK KEKE KEE KKK KKK KKK KKK KKK KER EE EE 


Boolean bufferClass::Get(BYTE& data) 


{ 
if (hasData()) { 
data = buf[getPtr]; 
inc(getPtr); 
return TRUE; 
} 
return FALSE; 
} 


ff PREEREEELERDEERE ER ERE SARE RS SERS ESSERE LERS A EELS AALS SS SS ee ee oe 


// FUNCTION NAME: Add 


// AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 

// DATE: 11 July 1995 

// DESCRIPTION: Writes a character to the buffer and checks for buffer 
ie J overflow 

// RETURNS: void 

// CALLS: hasData 

// CALLED BY: GPSbufferClass, compBufferClass 

// 


Se ee eK FT RK KT TOK KR KTR KKK KK KK KK KK RK KKK RK KEKE KER EK KEK KEKE KE EEK KEK KKK EEE 


void bufferClass::Add(BYTE ch) 


{ 
buf [putPtr] = ch; 
inc (putPtr); 
if (!thasData()) { // if no data after adding data, it overflowed 
cerr << "\nError: byteBuffer overflow\n"; 
} 
} 


// end of file buffer.cpp 
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D. GPSBUFF.H 


#ifndef _GPSBUFF_H 
#define _GPSBUFF_H 


#include "globals.h" 
#include "toetypes.h" 
#include "buffer.h" 


define GPSBLOCKS 4 
#define LINE _ FEED 10 
#define CARR_RETURN 13 


JEEEELEEAELELLELEALAELAE AA ERR ERLE LEER EERE RES ERASERS LES Se ee ee 


Class buffers GPS position messages via serial port communications. 
Uses a multiple buffer system in which each buffer is capable of 
holding a single position message. Buffers are filled and processed 
sequentially in a round robin fashion. Messages are checked for 
validity only upon attempted reads from the buffer. 


Bek KR RR KEK RR KR KR KR EK KKK RK RRR KR KEKE KK KER EK KKK KKK EKER EK KEE KKEKKE KEE KEKE KEKE KK / 
class gpsBufferClass : public bufferClass { 
public: 


gpsBufferClass (BYTE GPSblocks = GPSBLOCKS) ; 
~gpsBufferClass() { delete [] block; } 


Boolean hasData({); // a complete structure is ready 
Boolean Get (BYTE&) { return FALSE; } 
Boolean Get (GPSdata) ; // £i11 in a complete structure 
void Add (BYTE ch); // build the structure byte by byte 
protected: 
Boolean validHeader (GPSdata) ; // check a block for valid header 
GPSdata *block; // hold the buffered GPS data 
WORD current, last; // current and last GPS block in 
use 
BYTE *putPlace; // for the next character received 
}; 
#endif 
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E. GPSBUFF.CPP 


#include <iostream.h> 
#include <stdio.h> 


#include “gpsbuff.h" 


/ 


Bee Fe KK EC FERRE TOK BKK KE KK KK KKK RK KKK KER KK REI K EKER KEK EERE KEKE KKK KE REKEKEKEKREEEE 


PROGRAM : gpsBuffer (Constructor) 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Allocates message buffers, indicate that no data has been 
received by equalizing current and last and set position 
into which initial character will be read. 

RETURNS : nothing. 

CALLED BY: navigator class (nav.h) 

CALLS: none. 


a er ee ee ee ee ee ee ee ee ee ee ee ees 


gpsBufferClass: :gpsBufferClass (BYTE GPSblocks) : current(0), last(0), 
bufferClass(GPSblocks) // Call to base class constructor 
{ 
cerr << "constructing gpsBuffer" << endl; 
block = new GPSdata[GPSbiocks]; // Create an array of GPSdata elements 
putPlace = &(block[current][0]); // Set the place for first character 


[REEAEELRELEEL ELE RELAA ERE LEE AAA ERLE ELA ARES ERERE LEE ELLE ALES LE ROSSER 


PROGRAM: Add 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Interrupt driven routine which writes incoming characters 
into the gps buffers 

RETURNS : nothing. 

CALLED BY: interupt driven by bufferedSerialPort 

CALLS: none. 


ee ee RET TE KE KR KR KKK KEK KR KKK KKK KK RE KRKEKEKKE RR KE KK KEK KEKE KEKE KEKE KEKE KEKE E EK / 


void gpsBufferClass::Add(BYTE data) 
{ 


static BYTE lastChar (data) ; // Holds last for <cr> <lf> detection 
static Boolean 1fFlag = FALSE; // True when message end is detected 
if (lfFlag && (data == '@')) { // Is a new message starting? 
last = current; // Set last to buffer with newest message. 
ine (current) ; // Set current to the next buffer 
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// Set putPlacé to the beginning of the next buffer. 
putPlace = &(block[current][0]); 
lfFlag = FALSE; // reset for end of next message. 


*putPlace++ = data; // Write character into the buffer. 


//Has the end of a message been received? 

if ((lastChar == CARR_RETURN) && (data == LINE_FEED)) { 
lfFlag = TRUE; 

} 


lastChar = data; //Save last character for <cr> <lf> 
detection 


} 


LREEELEERRERAELEEAL ERE R SEERA ELE LERL EERE RELL ELLE EER ERE EA LALLA ES Se 


PROGRAM : Get 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Checks to see if a new message has arrived, copies it into 
the input argument data and returns a flag to indicate 
whether a new message was received. 

RETURNS : TRUE, if a new valid position has been received. 
FALSE, otherwise 

CALLED BY: navPosit (nav.cpp) 
initializeNavigator (nav.cpp) 

CALLS: gpsBufferClass: :hasData 


KKK RRR KEK KR KR RK RK KK KR KR KR KKK KEKE KKK EK KEK RE KKK KER KER EKKEKEKREKKEKEEKK KEK EEE / 


Boolean gpsBufferClass::Get(GPSdata data) 
{ 
if (hasData()) { // Has a new valid message been 
received. 
// Copy the message out of the buffer. 
memcpy (data, block + last, GPSBLOCKSIZE) ; 
last = current; // Indicate that this message has been read. 
return TRUE; 
} 
else { 
return FALSE; 
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PROGRAM: hasData 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Determines whether a new message has been received and 
checks to sée if it has a valid header. 


RETURNS : TRUE, if a new valid message has been received. 
CALLED BY: gpsBufferClass::Get (buffer.cpp) 
CALLS: validHeader (buffer.cpp) 


de te Fe Ke Ke te BKK KE KT KK TK KK KK IK TK KK IK KK KICK KK KK KK KKK KER KKK REE KEKE KK KEKE KEE KEKE KEK / 


Boolean gpsBufferClass: :hasData() 


{ 
// Has a new message with a valid header been received 
if (last != current) { 
if (validHeader(block[last])) { 
return TRUE; 
} 
else { 
return FALSE; 
} 
} 
return FALSE; 
} 


LEE EEEEEELEL RELA ER ELSE AEE LE LES LEER ERE SRA SRE eS EAS ESE LS See 


PROGRAM: validHeader 

AUTHOR: Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Checks to see if a message has the proper header for a 
Motorola position message. (@@Ea) 

RETURNS : TRUE, if the header is valid. FALSE, otherwise. 

CALLED BY: gpsBufferClass::hasData (buffer.cpp) 

CALLS: none. 


Bee ee KR KKK TKK KR KK KK TK KR KK KK KKK KEKE KK KEKE KEE EK KE REE ERK KE REEEKEEKEKEEKE / 


Boolean gpsBufferClass::validHeader(GPSdata dataPtr) 


{ : 
if ((dataPtr[0] == '@') && (dataPtr{1] == '@') && 
(dataPtr[2] == 'E') && (dataPtr[3] == 'a')) { 

return TRUE; 

} , 

else { 
return FALSE; 

} 


// end of file gpsbuff.cpp 
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COMPBUFF.H 


#ifndef __COMPBUFF_H 
#define __COMPBUFF_H 


#include "toetypes.h" 
#include "globals.h" 
#include “buffer.h" 


#define COMPBLOCKS 8 
#define LINE_FEED 10 
#define CARR_RETURN 13 
#define g 103 
#define o pi al Te 


LEEELESALELAA ER REAER EER LER AAAS RARER Se eh ee ee ee ee eh me a oem eee te ee 


Class buffers COMPASS messages received via serial port communications. 
Uses a multiple buffer system in which each buffer is capable of 
holding a single message. Buffers are filled and processed 


sequentially in a round robin fashion. Messages are checked for 


validity only upon attempted reads from the buffer. 


ak KK KR RR eR KR RK RR RR KR KK RR KR KK KKK KKK KERR KEKE KEK KKEKEKKEKEEE KK ERE KK EEK / 


class compBufferClass : public bufferClass { 


public: 


compBufferClass (BYTE compBlocks = COMPBLOCKS) ; 


~compBufferClass() {delete [] block;} 


Boolean hasData(); // a complete structure is ready 
Boolean Get(BYTE&) {return FALSE;} // satisfy inheritence 
Boolean Get (compData) ; // get a complete structure filled in 
void Add (BYTE ch); // build the structure byte by byte 
protected: // for inheritance 
Boolean validHeader (compData) ; // check a block for valid header 
compData *block; // points to array of compass msgs 
WORD current, last; // current and last comp block in use 
BYTE *putPlace; // for the next character received 
a 
#endif 
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G. COMPBUFF.CPP - 


#include <iostream.h> 
#tinclude <stdio.h>. 


#include "“compbuff.h" 


LEEEEERER ALAA LE EA ERASER ESE SEAT E LEIS SESE EL ERE AERRELA LL ERE SOR EA SE OE 


PROGRAM: compBuffer (Constructor) 
AUTHOR: Eric Bachmann, Randy Walker 
DATE: 28 April 1996 
FUNCTION: Allocates message buffers, indicates that no data has been 
received by equalizing current and last and sets the position 
into which initial character will be read. 


RETURNS: nothing. 
CALLED BY: compassClass (compass.h) 


CALLS: none. 


Bee tee ete ke de ee I OK te te RR KK RR RK KR RR KR RR RRR RK KK KK KK KR EK KK KEK K / 


compBufferClass::compBufferClass (BYTE compBlocks): current(0), last(0), 
bufferClass(compBlocks) // Call to base class constructor 


cerr << "compBuffer constructor called" << endl; 


block = new compData[compBlocks]; // Create array of message buffers 
putPlace = &(block[current][0]); // Set position for first character 


cerr << "“compBuffer constructed." << endl; 


[RRR EER RK REERAER EERE AH HEE EKER RE EER ERE EHH REE ERE HREE RK EE REEL ER EREEES 


PROGRAM: compBuffer: :Add 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 28 April 1996 

FUNCTION: Interrupt driven routine which writes incoming characters 
into the compass message buffers 


RETURNS : nothing. 
CALLED BY: interrupt driven by compassPort 
CALLS : none. 


de de ke Ke TE KK KKK TBR IK KK I KKK KKK KK IKK KKK KEK ERK KKK KKK KEKE KKK KEKE KEKE KEKE KEE / 
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void compBufferClass::Add(BYTE data) { 


static Boolean 1fFlag = FALSE; //True, if message end detected 

static int messageCount (0); // Counts characters in current message 

ahs (lfFlag && (data: == "%S")) 4 // Is a new message starting? 
last = current; // Set last to buffer with newest message. 
inc (current); // Set current to the next buffer 


// Set putPlace to the beginning of the next buffer. 
putPlace = &(block[current] [0]); 


lfFlag = FALSE; // reset for end of next message. 
} 
*putPlace++ = data; // Write character into the buffer. 
messageCount++; 


//Has the end of a message been received (<cr><lf>)? 
if (data == LINE_FEED) { 
lfFlag = TRUE; 


} 


[EEEEEELIAELLEE RIL ELELL ELE FESR ELLE LEAL ERASERS PERRET CREAN 


PROGRAM: compBuffer: :Get 

AUTHOR: Eric Bachmann, Randy Walker 

DATE: 28: April 1996 

FUNCTION: Checks to see if a new message has arrived, copies it 
into the input argument data and returns a flag to indicate 
whether a new message was received. 

RETURNS : TRUE, if a new valid position has been received. 
FALSE, otherwise 

CALLED BY: compass.cpp 

CALLS: compBuffer::hasData 


te eK KK KKK HH KK SRR KKK KK KK IKK KER KK KKK KKK EEK KEK KERR KK KEKE EERE REKE KEE / 


Boolean compBufferClass::Get(compData data) 
{ 
if (hasData()) { // Has a new valid message been received. 
// Copy the message out of the buffer. 
memcpy (data, block + last, COMPSIZE) ; 


last = current; // Indicate that this message has been read. 
return TRUE; 


} 


else { 
return FALSE; 
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PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 


RETURNS: 
CALLED BY: 
CALLS: 


compBuffer: :hasData 

Eric Bachmann, Randy Walker 

28 April 1996 

Determines whether a new message has been received and 
checks to see if it has a valid header. 

TRUE, if a new valid message has been received. 
compBuffer: :Get 

validHeader (compBuffer.cpp) 


Be Je Je eK KK IR IK KKK IK KKK TKK KK TR KO KK RK KK KE KKK KE RK KEK KEKE KKH KK KEE KE RREE ER / 


Boolean compBufferClass: :hasData() 


{ 


if ((last != current) && (validHeader(block[last]))) { 
return TRUE; 


} 


else { 


return FALSE; 


[BRI RI IIR I ERI RRR TR ERIK EEE RIKER ERE RE RE KEKE K AE EERE EE EES ES 


PROGRAM: 
AUTHOR: 
DATE: 
FUNCTION: 


RETURNS: 
CALLED BY: 
CALLS: 


validHeader 

Eric Bachmann, Dave Gay 

11 July 1995 

Checks to see if a message has the proper header for a 
compass message. (SC) 

TRUE, if the header is valid. FALSE, otherwise. 
compBuffer: :hasData 

none. 


Be dee Ea I Te rR TOR I AK a a RK IK KR KR KK I KR KKK KEK KEKE KKK KK ERK EKER / 


Boolean compBufferClass: :validHeader (compData dataPtr) 


{ 


if ((dataPtr[0] == '$') && (dataPtr[i] == 'C')) { 
return TRUE; 


} 


else { 


return FALSE; 


} 
} 


//end of file compbuff.cpp 
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H. SERIAL.H 


#ifndef _SERIAL_H 
#define _SERIAL H 


#include <dos.h> 

#include <stdio.h> 

#includé "globals.h" 

#define ALMOST_FULL 80 // % full to turn off DTR (user defines) 


// leave the following alone - hardware specific 


enum COMport {COM1=1, COM2, COM3, COM4}; 

enum BaudRate {b300, b1200, 62400, b4800, b9600}; 
enum ParityType {ERROR=-1, NOPARITY, ODD, EVEN}; 
enum handShake {NONE, RTS_CTS, XON_XOFF}; 

enum Shake {OET, On} 


enum interruptType ({rx_rdy, tx_rdy, line_stat, modem_stat}; 


#define BIOSMEMSEG 0x40 


#define DLAB 0x80 
#define IRQPORT 0x21 
#define EOL outporth (0x20, 0x20) 


#define COMibase MEMW (BIOSMEMSEG, 0) 
#define COM2base MEMW (BIOSMEMSEG, 2) 


#define TX (portBase) 

#define RX (portBase) 

#define IER (portBase+1) 
#define IIR (portBase+2) 
#define LCR (portBase+3) 
#define MCR (portBase+4) 
#define LSR (portBase+5) 
#define MSR (portBase+6) 
#define LO _LATCH (portBase) 

#define HI_LATCH (portBase+1) 


[REEL ELEELERA AERA EE ERAPLALS SAAT SAAS ASE CRRA AES Se A RRS Re ne ene 


CLASS: serialPortClass 

AUTHOR : Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995, last modified January 1997 

FUNCTION: Parent class, defines a simple serial port. 


Be KK KK KK KK TK KK IKK KK KKK KKK KKK KKK IKKE KKK KR KEK KK ERK REE KE KERR RK KEKE KEKE ERE ER / 


class serialPortClass { 


public: 
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serialPortClass(COMport port, BaudRate baud, ParityType parity, 
BYTE wordlen, BYTE stopbits, handShake hs) ; 


‘~serialPortClass() {} 
Boolean Send (BYTE data); 
Boolean Get (BYTE& data); 


inline Boolean dataReady (); 
Boolean statusChanged() 
{ return Boolean((ifportbit(MSR,0) I! ifportbit(MSR,1))); } 


// the rest are only if handshake is specified as RTS_CTS 


Boolean isCTSon () { return ifportbit(MSR,4); } 
Boolean isDSRon () { return ifportbit(MSR,5); } 
void setDTRon () { setportbit (MCR,0); } 

void setDTRoff () { clrportbit (MCR,0); } 
void toggleDTR(); 

void setRTSon () { setportbit(MCR,1); } 
void setRTSoff () { clrportbit (MCR,1); } 

void toggleRTS (); 

protected: 

WORD portBase; 

handShake ShakeType; 

Shake DTRstate, RTSstate; 


BAe 
#endif 


inline Boolean ifportbit (WORD, BYTE) ; 
inline void toggle (Shake&) ; 
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I. SERIAL.CPP 


#include <iostream.h> 
#include <stdio.h>. 
#include "serial.h" 


77 Usage Note: Because of the interrupt handlers used, you MUST call 
// your compassPort & gpsPort objects port2 & portl so the 
iJ right handler gets called and can properly service the interrupt. 


LREEALLEREREERESELE LE LES ERAS LLERE RAE RAE EL ES EAL ES SSE SELES SEALE ELA RES 


PROGRAM: serialPortClass (Constructor) 
AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995, last modified January 1997 


FUNCTION: Initializes one of the Serial Ports. 
1) Determines the base I/O port address for the given COM port 
2) Sets the 8259 IRQ mask value 
3) Initializes the port parameters - baud, parity, etc. 
4) Calls the routine to initialize interrupt handling 
5) Enables DTR and RTS, indicating ready to go 


KKK TKK KK KR KR KKK KR KKK KEK EK KK KEK KK KE KEK RE KKEKKEKEKEEKEKKE KEE ER KEK KEK KEK REKER / 


serialPortClass::serialPortClass(COMport port, BaudRate speed, 
ParityType parity, BYTE wordlen, 
BYTE stopbits, handShake hs) 
DTRstate(off), RTSstate(off), ShakeType (hs) 


cerr << "serialPort constructor called" << endl; 


delay (500); 

switch (port) { // initialize appropriate port base 

case COM1: portBase = COMlbase; 
break; 

case COM2: portBase = COM2base; 
break; 

} // switch 

const WORD bauddiv[] = {0x180, 0x60, 0x30, 0x18, OxC}; 


// Change 1 

outportb(IER, 0); // disable UART interrupts 
(void) inportb(LSR) ; 

(void) inportb(MSR) ; 

(void) inportb(IIR); 

(void) inportb (RX); 

outportb(LCR,DLAB); // set DLAB so can set baud rate (read only port) 
outportb(LO_LATCH, bauddiv[speed] & OxFF); 

outporth(HI_LATCH, (bauddiv[{speed] & OxFF00) >> 8); 

setportbit (MCR,3); // turn OUT2 on 
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BYTE. opt = 0; 
if (parity != NOPARITY) { 
setbit (opt,3); // enable parity 
if (parity == EVEN) // set even parity bit. if odd, leave bit 0 
setbit(opt,4); 
} 
// new set the word length. len of 5 sets both bits 0 and 1 to 
// 0, 6 sets to 01; “7 -€6- 10: ana ‘2 to- 11 
opt |= wordlen-5; 
opt |= --stopbits << 2; 
outportb(LCR,opt) ; 


if (ShakeType == RTS_CTS) { 
setDTRon(); 
setRTSon (); 

} 


cerr << "serialPort constructed" << endl; 


[SEEEERELLLELRERER LEELA ES DEAS EASA LER SAREE ELE SARS EEE eee ee en 


PROGRAM : Get 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Gets a byte from the port. Returns true if there's one 
there, and fills in the byte parameter. If there's no 
character, the parameter is left alone, and false is 
returned. 


ee ee ee ee ee ee ee ee ee ee eee ee ee ee ie | 


Boolean serialPortClass::Get(BYTE& data) 


{ 





if (dataReady()) { // make sure there's a char there 
data = inportb(RX); // read character from 8250 
return TRUE; 

} 

else 
return FALSE; 
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PROGRAM: Send 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Outputs a single character to the port. Returns Boolean 
status indicating whether successful 
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Boolean serialPortClass::Send(BYTE data) 


{ 
while (!(ifportbit(LSR,5))) {}; // wait until THR ready 
switch (ShakeType) { 
case NONE: 
outportb (TX, data); 
return TRUE; 
case RTS_CTS: 
if (isCTSon() && isDSRon()) { 
outportb (TX, data) ; 
return TRUE; 
} 
else { 
return FALSE; 
} 
// case XON_XOFF: // add this later if needed 
default: 
break; 
} 
return FALSE; 
} 


[EERE EEE EE EEAEEES EEL ERE ELE LAL EEE RREEEELES LE RERELAEARLAA EELS ERLE 


PROGRAM: dataReady 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Checks port to see if a character has arrived. 


KKK IKK KKK KE RK RK KEK KEKE KKK KEK KEK KEKE KEE KEE KKK EK KEK KEK KEKEKEKE KEKE KKEKEEEKKKKERK / 
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inline Boolean serialPortClass: :dataReady () 


{ 
j* ' Un-commenting this code increases transmission errors, but this 
code is useful for troubleshooting, so is retained if needed 
1£ (ifportbit(LSR,1)) { 
cerr <<"\nOverrun Error\n"; 
} 
if ¢ifportbit(LSR,2)) { 
cerr <<"\nParity Error\n"; 
} 
if (ifportbit(LSR,3)) { 
cerr <<"\nFraming Error\n"; 
} 
<7 


return ifportbit(LSR, 0); 
} 


LLELELREARRAEAREEEREL AEA ALA ELLE LARA AEL AERA ALARA EALAA ALLA EA REALS SRE EET 


PROGRAM: ifportbit 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Checks for byte on inporthb register 
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inline Boolean serialPortClass::ifportbit (WORD reg, BYTE bit) 
{ 
BYTE on = inportb(reg); 
on &= set(bit); 
return Boolean(on == set(bit)); 


} 


[RR KKK KH KH KKH HK KE KEKE KEKE KKK KE KKK KEE KEE RE REA EKRERRRER EEK ARE EEE 


PROGRAM: toggleDTR 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: toggles Data Transmit Ready if RTS_CTS is off 


Ke ee a KK KK KKK KKK REE KEKE KK ERE EKER KK KEKE KKK KEK RE RK EREE EK / 


void serialPortClass: :toggleDTR() 
{ 
if (ShakeType != RTS_CTS) 
return; 
if (DTRstate == off) 
setDTRon (); 
else 
setDTRoff(); 
teggle(DTRstate) ; 
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PROGRAM: toggleRTS 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: toggle Ready to Send (RTS) if RTS_CTS is on. 
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void serialPortClass::toggleRTS() 
{ 
if (ShakeType != RTS_CTS) 
return; 
if (RTSstate == off) 
setRTSon(); 
else 
setRTSof£f(); 
toggle (RTSstate) ; 
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PROGRAM: toggle 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: LL wuly 1995 

FUNCTION: toggles value of the input variable 
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inline void serialPortClass::toggle(Shake& h) 


{ 
if (h == off) 
h = on: 
else 
fh. = off; 
} 


// end of file serial.cpp 
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J. GPSPORT.H 


#ifndef _GPSPORT_H 
#define _GPSPORT_H 


#include <dos.h> 
#include <stdio.h> 
#include “toetypes.h" 
#include "“globals.h" 
#include "serial.h" 
#include “gpsbuff.h" 


// this is the type for a standard interrupt handler 
typedef void interrupt (IntHandlerType) (..-); 


// com handler to interface with processInterrupt 
void interrupt COMlhandler(...); 


LELELLE SAE EL EL OLEE LEAL LEAL AE ELE AAR SEE EAM ARAL ASS RDA RS ONS ee ee Bes 


CLASS:gpsPortClass 
AUTHOR:Rick Roberts 
DATE:28 January 1997 


FUNCTION: Defines a buffered serial port which is interrupt driven 
on receive, and buffers all incoming characters in the 
gps buffer 


Bete KK KR KKK KKK KKK KKK KR REE KKK KEK KKK KEKE KEKE KKK KEKE KKK KK KEK KKK KREEKKEREK EEE / 


class gpsPortClass : public serialPortClass { 
public: 


gpsPortClass(COMport portnum = COM1, BYTE irg = 4, 
BaudRate speed = 69600, 
ParityType parity = NOPARITY, BYTE wordlen = 8, 
BYTE stopbits = l, 
handShake hs = XON_XOFF) ; 


~gpsPortClass(); 

Boolean Get (GPSdata& data); // buffered version 

void processInterrupt(); // buffers the incoming character 
protected: 


gpsBufferclass messages; 


BYTE irgbit; // Value to allow enable PIC interrupts for COM port 
BYTE origirg; // keep the original 8259 mask register value 
BYTE comint; 
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IntHandlerType *origcomint; // keep original vector for restoring 
// ljater 


// this allows the actual handler to access processInterrupt () 
friend IntHandlerType COM2handler; 


}; 
extern gpsPortClass portl; 


#endif 


K. GPSPORT.CPP 


#include <iostream.h> 
#include <stdio.h> 
#include "gpsPort-.h" 


[FELL ELEARLAA LLL EELS EL AAARRAEREA LER RAELERLL ALAA AA EAE ALS AEE SS RRR 


PROGRAM : gpsPortClass (Constructor) 
AUTHOR: Rick Roberts 
DATE: 28 January 1997 


FUNCTION: Initializes the interrupts for the gps Serial Port. 
1) takes over the original COM interrupt 
2) sets the port bits, parity, and stop bit 
3) enables interrupts on the 8250 (async chip) 
4) enables the asyne interrupt on the 8259 PIC 


Perr rr rr Stee eee eee eee eee ee ee ee er 


gpsPortClass::gpsPortClass(COMport portnum, BYTE irg, BaudRate baud, 
ParityType parity, BYTE wordlen, 
BYTE stopbits, handShake hs) 
serialPortClass(portnum, baud, parity, wordlen, 
stopbits, hs), 
irgbit(irg), comint (irgqbit+8) 


cerr << "“gpsPort constructor called" << endl; 
if (ShakeType == RTS_cCTS) { // turn it off first, it was enabled 
setDTRoff(); // in the base class 


setRTSoff(); 
} 


origcomint = getvect (comint) ; // remember the original vector 
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setvect (comint, COMlhandler) ; // point to the new handler 


setportbit (MCR, 3); // turn OUT2 on 
disable(); // Gisable all interrupts - critical section 
setportbit (IER, rx_rdy) ; // enable ints on receive only 


origirg = inportb(IRQPORT); // remember how it was 
clrportbit (IRQPORT,irgbit); // enable COM ints 


if (ShakeType == RTS_cCTS) { 
setDTRon(); 
setRTSon(); 

} 


enable({); 


EOI; 
cerr << “exiting gpsPort constructor" << endl; 


[RPEEELEEE PRESSES AEE ELISE REL ELE AE AES ELE SEES EL Re eee em ee tiene ee 


PROGRAM: ~gpsPortClass 
AUTHOR: Rick Roberts, Frank Kelbe, Eric Bachmann, Dave Gay 


DATE: 28 January 1997 

FUNCTION: Resets the interrupts. 
1) disables the 8250 (async chip) 
2) disables the interrupt chip for asyne int 
3) resets the 8259 PIC 


eK KK IIR TKK TK KK KR KKK KK KKK KR EKER KKK KEE EK ERK KKKKEEEKKREKEKKEEEEE / 


gpsPortClass::~gpsPortClass() 
{ 


setvect (comint,origcomint) ; // set the interrupt vector back 
outportb(IER, 0); // Aisable further UVART interrupts 
outportb (MCR, 0) ; // turn everything off 
outportb(IRQPORT, origirg); 

EOL; 


JEEEEREER DERE S EEAREDEA ALLE EAA EAK EAE EAS EA RIOR EIS Be ee ee a ee 


PROGRAM: Get 
AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 


DATE: 11 July 1995 
FUNCTION: Calls Get based on buffer type 


Be ke ee te ted RR KK RK KK IK KK RIK KE KKK KEKE KKK KK KEK IKKE EK KER ERK ERK EKER RRKKKE KEKE / 
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Boolean gpsPortClass::Get(GPSdata& data) 
{ 


return messages .Get (data) ; 


[FREER REEREREA EE ERERE LS ER ELA ELLER ER ERE RARER EAEAELERA ESLER LAER AE AER ERE SRE 


PROGRAM : showPorts 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Prints interrupt vector addresses. This function is for 


trouble shooting, it is not called in the code. 


eK KK IKK KK KKK KKK KR KR KKK KR KKK RE KK KKK EKER KHER EEK K ERE KKEKKKEKRKE KEKE EKER KEKE E / 


{* 

int showPorts() 

{ 
BYTE* p = (BYTE*)COM2base; 
Dp += “53 
fprintf(stderr, "SX “ *p+4+); 
fprintf(stderr, "SX\n", *p++) ; 
fprintf(stderr, "IRQPORT = %X", inportb(IRQPORT) ); 
return 0; 

} 


LRELAEREAEEARERLEELERLAE LEELA ERE RELL REALE EEE RL ERLE ESE ALLER ESE 


PROGRAM : COMihandler 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995, last modified January 1997 

FUNCTION: Specific interrupt handler which maps each interrupt to 


the proper ISR. 


KKK KK RK IK KK KKK RRR KKK KEKE KR IKKE KEKE RR EK KEE KEK KK KE KKK KK KK KKK EEEKEEKEKERE EE / 


void interrupt COMlihandler(...) 
{ 


portl.processInterrupt (); 
EOL: 
} | 
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PROGRAM: processInterrupt 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995 

FUNCTION: Calls the ISR based upon buffer type 


KKK RK KK KEK KK KER KKK KEKE EKER KKK KEK RRR KKK KER KEKE EK KK RKEK EK KKEKEEK KEKE KKK KR KEEK EK / 
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void gpsPortClass: :processInterrupt () 


( 


if (dataReady()) {¢ // make sure there's a char there 
BYTE data = inportb(RX); // read character from 8250 
messages .Add(data) ; 

if (ShakeType == RTS_CTS && messages.capacityUsed() > ALMOST_FULL) 


setDTRoff(); 


} 


} : 
// end of file gpsport.cpp 


L. COMPPORT.H 


#ifndef _COMPORT_H 
#define _COMPORT_H 


#include <dos.h> 
#include <stdio.h> 
#include "toetypes.h" 
#include "“globals.h" 
#include “serial.h" 
#include “compbuff.h" 


// this is the type for a standard interrupt handler 
typedef void interrupt (IntHandlerType) (...)j 


// com handler to interface with processInterrupt 
void interrupt COM2handler(...); 


[LREELELEAEREELER AER EL EEELLLEES LRELLEE REL EE REA A EERALAL EERE RR EEA EEE EAE TES 


CLASS: compassPortClass 

AUTHOR: Rick Roberts 

DATE: 28 January 1997 

FUNCTION: Defines a buffered serial port which is interrupt driven 


on receive, and buffers all incoming characters in the 
compass buffer 


a Se ee er re Se ee ee ee ee, 
class compassPortClass : public serialPortClass ({ 
friend compassClass; 
public: 
compassPortClass(COMport portnum = COM2, BYTE ire. = 3; 
BaudRate speed = b9600, 


ParityType parity = NOPARITY, BYTE wordlen = 8, 
BYTE stopbits = 1, handShake hs = NONE); 
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~compassPortClass(); 


Boolean Get (BYTE& data); // buffered version 
void processInterrupt(); // buffers the incoming character 
private: 


compBufferClass headings; 


BYTE irgbit; // Value to allow enable PIC interrupts for COM port 
BYTE origirg; // keep the original 8259 mask register value 
BYTE comint; 


IntHandlerType *origcomint; // keep original vector for restoring 
// later 


// this allows the actual handler to access processInterrupt () 
friend IntHandlerType COMZ2handler; 


}; 
extern compassPortClass port2; 


#endif 


M. COMPPORT.CPP 


#include <iostream.h> 
#include “compport.h" 


fLEREEEEELELL EEE RAE LES LE LAE LEER RER AEE SA ALE RELEASE AERA ALS ERA ICR SRI SE 


PROGRAM: compassPortClass (Constructor) 
AUTHOR: Rick Roberts 
DATE: 28 January 1997 


FUNCTION: Initializes the interrupts for the compass Serial Port. 
1) takes over the original COM interrupt 
2) sets the port bits, parity, and stop bit 
3) enables interrupts on the 8250 (async chip) 
4) enables the async interrupt on the 8259 PIC 


KK KK KK KR KKK KKK KK KKK KR KE KKK KR KEK KKK KKK KKK KKK KKK KEKE KKK KEK KEE KEKE ERE KKK EK / 


compassPortClass::compassPortClass(COMport portnum, BYTE irq, 
BaudRate baud, ParityType 
parity, BYTE wordlen, BYTE 
stopbits, handShake hs) : 
serialPortClass(portnum, baud, parity, wordlen, 
stopbits, hs) 
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cerr << "compassPort constructor called" << endl; 


Lrobilt: =: 2rq- 

comint = i1rqbit + 8; 

if (ShakeType == RTS_CTS) ({ // turn it off first, it was enabled 
setDTRoff(); // in the base class 


setRTSoff(); 
} 


origcomint = getvect(comint) ; // remember the original vector 
setvect (comint, COM2Zhandler) ; // point to the new handler 


setportbit (MCR,3); // turn OUT2 on 

disable(); // disable all interrupts - critical section 
setportbit (IER, rx_rdy) ; // enable ints on receive only 
origirg = inportb(IRQPORT); // remember how it was 
clrportbit (IRQPORT,irgbit); // enable COM ints 


if (ShakeType == RTS_CTS) { 
setDTRon(); 
setRTSon(); 

} 


enable(); 


EOI; 
cerr << "exiting compassPort constructor" << endl; 


} 


[EEREREEERELAE REEL REBEL RAL EEE AREER EAA ES ERS AE ELLELEEE ALA LES SORE 


PROGRAM: ~compassPort 
AUTHOR: Rick Roberts, Frank Kelbe, Eric Bachmann, Dave Gay 
DATE : 28 January 1997 


FUNCTION: Resets the interrupts. 
1) disables the 8250 (async chip) 
2) disables the interrupt chip for async int 
3) resets the 8259 PIC 


KK KK KK KR KKK KEK KKK KR KR KEK KKK KE ER KKK EK KEKE KKK KER KEKE KEKE KKKEKRKEK EERE KK / 


compassPortClass::~compassPortClass () 


{ 


setvect (comint, origcomint) ; // set the interrupt vector back 
outportb(IER,0); // disable further UART interrupts 
outportb (MCR, 0); | // turn everything off 
outportb(IRQPORT, origirg) ; 

EOL; 
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PROGRAM: Get 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 
DATE: 11 July 1995 

FUNCTION: Calls Get based on buffer type 
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Boolean compassPortClass::Get (BYTE& data) 
{ 


return headings.Get (data) ; 


JRLEEEELAALEEAER LEAR A ALE ASE AR AER ASAE EAA SELES LAR AL AAA EE EAT TS ee ee 


PROGRAM: showPorts 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay 

DATE: 11 July 1995 

FUNCTION: Prints interrupt vector addresses. This function is for 


trouble shooting and is not called from the code. 


Be KKK IK ITT KK KK KR KKK KKK KK KKK KR KK KR KKK KK KKK KK KEKE KKK KKK KKK KKK KE REE RK K / 


/* 
int showPorts() 
{ 
BYTE* p = (BYTE*)COM2base; 
D +F]05% 
fprintf(stderr, "3X Nt Ore) 


fprintf(stderr, "SX\n", *p++) ; 
fprintf(stderr, "IRQPORT = SX", inportb(IRQPORT) ); 
return 0; 

} 

La 
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PROGRAM: COM2handler 

AUTHOR:Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 

DATE:11 July 1995, last modified January 1997 

FUNCTION: Specific interrupt handler which maps each interrupt to 
the proper ISR. 
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void interrupt COMZhandler(...) 
{ 


port2.processInterrupt (); 
EOI; 
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PROGRAM : processInterrupt 

AUTHOR: Frank Kelbe, Eric Bachmann, Dave Gay, Rick Roberts 
DATE: 11 July 1995 

FUNCTION: Calls the ISR based upon buffer type 
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void compassPortClass: :processInterrupt () 


{ 


if (dataReady()) { // make sure there's a char there 
BYTE data = inportb(RX) ; // read character from 8250 
headings .Add (data) ; 

if (ShakeType == RTS_CTS && headings.capacityUsed() > ALMOST_FULL) 
setDTRoff(); 

} 


} 
// end of file compport.cpp 
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APPENDIX C. SANS TILT-TABLE TEST TUNING AND 
CALIBRATION PROCEDURE 


Isolate Accelerometer Input From Integrator 


--Set K, to zero. 


--Only angular rate scale factor and bias effects will be reflected in error 


Choose Initial Bias Weight (biasWght) 


--Using project experience, background theory 


Determine Angular Rate Scale Factor 


--Baseline setting is 1.0. 

' --Adjust by determining SANS output vs. actual angle excursion. 
--Apply ratio to current scale factor to obtain corrected scale factor. 
--Commanded tilt table angles taken as truth 

--Scale factor adjusts the output of the IMU to actual tilt results. 


--pScale (roll), qScale (pitch) rScale (yaw) 


Adjust Gain Value Above Zero 


--Re-includes accelerometer input to filter 


Determine Accelerometer Scale Factor 


--Same process as angular rate scale factor 


--xAccelScale (pitch), yAccelScale (roll), zAccelScale(yaw) 


Fine Tuning 


--Adjust various factors from 1-5 above 
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